// Copyright (C) 2004, 2011 International Business Machines and others.
// All Rights Reserved.
// This code is published under the Eclipse Public License.
//
// Authors:  Carl Laird, Andreas Waechter     IBM    2004-08-13

#include "IpIpoptCalculatedQuantities.hpp"
#include "IpSumSymMatrix.hpp"
#include "IpLowRankUpdateSymMatrix.hpp"
#include "IpRestoIpoptNLP.hpp"

#include <cmath>
#include <limits>

namespace Ipopt
{
#if IPOPT_VERBOSITY > 0
static const Index dbg_verbosity = 0;
#endif

IpoptCalculatedQuantities::IpoptCalculatedQuantities(
   const SmartPtr<IpoptNLP>&  ip_nlp,
   const SmartPtr<IpoptData>& ip_data
)
   : ip_nlp_(ip_nlp),
     ip_data_(ip_data),

     curr_slack_x_L_cache_(1),
     curr_slack_x_U_cache_(1),
     curr_slack_s_L_cache_(1),
     curr_slack_s_U_cache_(1),
     trial_slack_x_L_cache_(1),
     trial_slack_x_U_cache_(1),
     trial_slack_s_L_cache_(1),
     trial_slack_s_U_cache_(1),
     num_adjusted_slack_x_L_(0),
     num_adjusted_slack_x_U_(0),
     num_adjusted_slack_s_L_(0),
     num_adjusted_slack_s_U_(0),

     curr_f_cache_(2),
     trial_f_cache_(5),
     curr_grad_f_cache_(2),
     trial_grad_f_cache_(1),

     curr_barrier_obj_cache_(2),
     trial_barrier_obj_cache_(5),
     curr_grad_barrier_obj_x_cache_(1),
     curr_grad_barrier_obj_s_cache_(1),
     grad_kappa_times_damping_x_cache_(1),
     grad_kappa_times_damping_s_cache_(1),

     curr_c_cache_(1),
     trial_c_cache_(2),
     curr_d_cache_(1),
     trial_d_cache_(2),
     curr_d_minus_s_cache_(1),
     trial_d_minus_s_cache_(1),
     curr_jac_c_cache_(1),
     trial_jac_c_cache_(1),
     curr_jac_d_cache_(1),
     trial_jac_d_cache_(1),
     curr_jac_cT_times_vec_cache_(2),
     trial_jac_cT_times_vec_cache_(1),
     curr_jac_dT_times_vec_cache_(2),
     trial_jac_dT_times_vec_cache_(1),
     curr_jac_c_times_vec_cache_(1),
     curr_jac_d_times_vec_cache_(1),
     curr_constraint_violation_cache_(2),
     trial_constraint_violation_cache_(5),
     curr_nlp_constraint_violation_cache_(3),
     unscaled_curr_nlp_constraint_violation_cache_(3),
     unscaled_trial_nlp_constraint_violation_cache_(3),

     curr_exact_hessian_cache_(1),

     curr_grad_lag_x_cache_(1),
     trial_grad_lag_x_cache_(1),
     curr_grad_lag_s_cache_(1),
     trial_grad_lag_s_cache_(1),
     curr_grad_lag_with_damping_x_cache_(0),
     curr_grad_lag_with_damping_s_cache_(0),
     curr_compl_x_L_cache_(1),
     curr_compl_x_U_cache_(1),
     curr_compl_s_L_cache_(1),
     curr_compl_s_U_cache_(1),
     trial_compl_x_L_cache_(1),
     trial_compl_x_U_cache_(1),
     trial_compl_s_L_cache_(1),
     trial_compl_s_U_cache_(1),
     curr_relaxed_compl_x_L_cache_(1),
     curr_relaxed_compl_x_U_cache_(1),
     curr_relaxed_compl_s_L_cache_(1),
     curr_relaxed_compl_s_U_cache_(1),
     curr_primal_infeasibility_cache_(3),
     trial_primal_infeasibility_cache_(3),
     curr_dual_infeasibility_cache_(3),
     trial_dual_infeasibility_cache_(3),
     unscaled_curr_dual_infeasibility_cache_(3),
     curr_complementarity_cache_(6),
     trial_complementarity_cache_(6),
     curr_centrality_measure_cache_(1),
     curr_nlp_error_cache_(1),
     unscaled_curr_nlp_error_cache_(1),
     curr_barrier_error_cache_(1),
     curr_primal_dual_system_error_cache_(1),
     trial_primal_dual_system_error_cache_(3),

     primal_frac_to_the_bound_cache_(5),
     dual_frac_to_the_bound_cache_(5),

     curr_sigma_x_cache_(1),
     curr_sigma_s_cache_(1),

     curr_avrg_compl_cache_(1),
     trial_avrg_compl_cache_(1),
     curr_gradBarrTDelta_cache_(1),

     dampind_x_L_(NULL),
     dampind_x_U_(NULL),
     dampind_s_L_(NULL),
     dampind_s_U_(NULL),

     initialize_called_(false)
{
   DBG_START_METH("IpoptCalculatedQuantities::IpoptCalculatedQuantities",
                  dbg_verbosity);
   DBG_ASSERT(IsValid(ip_nlp_) && IsValid(ip_data_));
}

IpoptCalculatedQuantities::~IpoptCalculatedQuantities()
{ }

void IpoptCalculatedQuantities::RegisterOptions(
   SmartPtr<RegisteredOptions> roptions
)
{
   roptions->SetRegisteringCategory("Convergence");
   roptions->AddLowerBoundedNumberOption(
      "s_max",
      "Scaling threshold for the NLP error.",
      0., true,
      100.,
      "(See paragraph after Eqn. (6) in the implementation paper.)");

   roptions->SetRegisteringCategory("NLP");
   roptions->AddLowerBoundedNumberOption(
      "kappa_d",
      "Weight for linear damping term (to handle one-sided bounds).",
      0., false,
      1e-5,
      "(see Section 3.7 in implementation paper.)");

   roptions->SetRegisteringCategory("Line Search");
   roptions->AddLowerBoundedNumberOption(
      "slack_move",
      "Correction size for very small slacks.",
      0.0, false,
      pow(std::numeric_limits<double>::epsilon(), 0.75),
      "Due to numerical issues or the lack of an interior, the slack variables might become very small. "
      "If a slack becomes very small compared to machine precision, the corresponding bound is moved slightly. "
      "This parameter determines how large the move should be. "
      "Its default value is mach_eps^{3/4}. "
      "(See also end of Section 3.5 in implementation paper - but actual implementation might be somewhat different.)");
   roptions->SetRegisteringCategory("Line Search");
   roptions->AddStringOption3(
      "constraint_violation_norm_type",
      "Norm to be used for the constraint violation in the line search.",
      "1-norm",
      "1-norm", "use the 1-norm",
      "2-norm", "use the 2-norm",
      "max-norm", "use the infinity norm",
      "Determines which norm should be used when the algorithm computes the constraint violation in the line search.");
}

bool IpoptCalculatedQuantities::Initialize(
   const Journalist&  jnlst,
   const OptionsList& options,
   const std::string& prefix
)
{
   std::string svalue;
   Index enum_int;

   options.GetNumericValue("s_max", s_max_, prefix);
   options.GetNumericValue("kappa_d", kappa_d_, prefix);
   options.GetNumericValue("slack_move", slack_move_, prefix);
   options.GetEnumValue("constraint_violation_norm_type", enum_int, prefix);
   constr_viol_normtype_ = ENormType(enum_int);
   // The following option is registered by OrigIpoptNLP
   options.GetBoolValue("warm_start_same_structure", warm_start_same_structure_, prefix);
   options.GetNumericValue("mu_target", mu_target_, prefix);

   if( !warm_start_same_structure_ )
   {
      dampind_x_L_ = NULL;
      dampind_x_U_ = NULL;
      dampind_s_L_ = NULL;
      dampind_s_U_ = NULL;

      tmp_x_ = NULL;
      tmp_s_ = NULL;
      tmp_c_ = NULL;
      tmp_d_ = NULL;
      tmp_x_L_ = NULL;
      tmp_x_U_ = NULL;
      tmp_s_L_ = NULL;
      tmp_s_U_ = NULL;
   }

   num_adjusted_slack_x_L_ = 0;
   num_adjusted_slack_x_U_ = 0;
   num_adjusted_slack_s_L_ = 0;
   num_adjusted_slack_s_U_ = 0;

   initialize_called_ = true;

   bool retval = true;
   if( IsValid(add_cq_) )
   {
      retval = add_cq_->Initialize(jnlst, options, prefix);
   }
   return retval;
}

///////////////////////////////////////////////////////////////////////////
//                         Slack Calculations                            //
///////////////////////////////////////////////////////////////////////////

SmartPtr<Vector> IpoptCalculatedQuantities::CalcSlack_L(
   const Matrix& P,
   const Vector& x,
   const Vector& x_bound
)
{
   DBG_START_METH("IpoptCalculatedQuantities::CalcSlack_L",
                  dbg_verbosity);
   SmartPtr<Vector> result;
   result = x_bound.MakeNew();
   result->Copy(x_bound);
   P.TransMultVector(1.0, x, -1.0, *result);
   return result;
}

SmartPtr<Vector> IpoptCalculatedQuantities::CalcSlack_U(
   const Matrix& P,
   const Vector& x,
   const Vector& x_bound
)
{
   DBG_START_METH("IpoptCalculatedQuantities::CalcSlack_U",
                  dbg_verbosity);
   SmartPtr<Vector> result;
   result = x_bound.MakeNew();
   result->Copy(x_bound);
   P.TransMultVector(-1.0, x, 1.0, *result);
   return result;
}

SmartPtr<const Vector> IpoptCalculatedQuantities::curr_slack_x_L()
{
   DBG_START_METH("IpoptCalculatedQuantities::curr_slack_x_L()",
                  dbg_verbosity);
   SmartPtr<Vector> result;
   SmartPtr<const Vector> x = ip_data_->curr()->x();
   SmartPtr<const Vector> x_bound = ip_nlp_->x_L();
   if( !curr_slack_x_L_cache_.GetCachedResult1Dep(result, *x) )
   {
      if( !trial_slack_x_L_cache_.GetCachedResult1Dep(result, *x) )
      {
         SmartPtr<const Matrix> P = ip_nlp_->Px_L();
         DBG_PRINT_VECTOR(2, "x_L", *x_bound);
         result = CalcSlack_L(*P, *x, *x_bound);
         DBG_ASSERT(num_adjusted_slack_x_L_ == 0);
         num_adjusted_slack_x_L_ = CalculateSafeSlack(result, x_bound, x, ip_data_->curr()->z_L());
      }
      curr_slack_x_L_cache_.AddCachedResult1Dep(result, *x);
   }
   return ConstPtr(result);
}

SmartPtr<const Vector> IpoptCalculatedQuantities::curr_slack_x_U()
{
   DBG_START_METH("IpoptCalculatedQuantities::curr_slack_x_U()",
                  dbg_verbosity);

   SmartPtr<Vector> result;
   SmartPtr<const Vector> x = ip_data_->curr()->x();
   SmartPtr<const Vector> x_bound = ip_nlp_->x_U();
   if( !curr_slack_x_U_cache_.GetCachedResult1Dep(result, *x) )
   {
      if( !trial_slack_x_U_cache_.GetCachedResult1Dep(result, *x) )
      {
         SmartPtr<const Matrix> P = ip_nlp_->Px_U();
         result = CalcSlack_U(*P, *x, *x_bound);
         DBG_ASSERT(num_adjusted_slack_x_U_ == 0);
         num_adjusted_slack_x_U_ = CalculateSafeSlack(result, x_bound, x, ip_data_->curr()->z_U());
      }
      curr_slack_x_U_cache_.AddCachedResult1Dep(result, *x);
   }
   return ConstPtr(result);
}

SmartPtr<const Vector> IpoptCalculatedQuantities::curr_slack_s_L()
{
   DBG_START_METH("IpoptCalculatedQuantities::curr_slack_s_L()",
                  dbg_verbosity);

   SmartPtr<Vector> result;
   SmartPtr<const Vector> s = ip_data_->curr()->s();
   SmartPtr<const Vector> s_bound = ip_nlp_->d_L();
   if( !curr_slack_s_L_cache_.GetCachedResult1Dep(result, *s) )
   {
      if( !trial_slack_s_L_cache_.GetCachedResult1Dep(result, *s) )
      {
         SmartPtr<const Matrix> P = ip_nlp_->Pd_L();
         result = CalcSlack_L(*P, *s, *s_bound);
         DBG_ASSERT(num_adjusted_slack_s_L_ == 0);
         num_adjusted_slack_s_L_ = CalculateSafeSlack(result, s_bound, s, ip_data_->curr()->v_L());
      }
      curr_slack_s_L_cache_.AddCachedResult1Dep(result, *s);
   }
   return ConstPtr(result);
}

SmartPtr<const Vector> IpoptCalculatedQuantities::curr_slack_s_U()
{
   DBG_START_METH("IpoptCalculatedQuantities::curr_slack_s_U()",
                  dbg_verbosity);

   SmartPtr<Vector> result;
   SmartPtr<const Vector> s = ip_data_->curr()->s();
   SmartPtr<const Vector> s_bound = ip_nlp_->d_U();
   if( !curr_slack_s_U_cache_.GetCachedResult1Dep(result, *s) )
   {
      if( !trial_slack_s_U_cache_.GetCachedResult1Dep(result, *s) )
      {
         SmartPtr<const Matrix> P = ip_nlp_->Pd_U();
         result = CalcSlack_U(*P, *s, *s_bound);
         DBG_ASSERT(num_adjusted_slack_s_U_ == 0);
         num_adjusted_slack_s_U_ = CalculateSafeSlack(result, s_bound, s, ip_data_->curr()->v_U());
         DBG_PRINT_VECTOR(2, "result", *result);
         DBG_PRINT((1, "num_adjusted_slack_s_U = %d\n", num_adjusted_slack_s_U_));
      }
      curr_slack_s_U_cache_.AddCachedResult1Dep(result, *s);
   }
   return ConstPtr(result);
}

SmartPtr<const Vector> IpoptCalculatedQuantities::trial_slack_x_L()
{
   DBG_START_METH("IpoptCalculatedQuantities::trial_slack_x_L()",
                  dbg_verbosity);

   num_adjusted_slack_x_L_ = 0;
   SmartPtr<Vector> result;
   SmartPtr<const Vector> x = ip_data_->trial()->x();
   SmartPtr<const Vector> x_bound = ip_nlp_->x_L();
   if( !trial_slack_x_L_cache_.GetCachedResult1Dep(result, *x) )
   {
      if( !curr_slack_x_L_cache_.GetCachedResult1Dep(result, *x) )
      {
         SmartPtr<const Matrix> P = ip_nlp_->Px_L();
         result = CalcSlack_L(*P, *x, *x_bound);
         DBG_ASSERT(num_adjusted_slack_x_L_ == 0);
         num_adjusted_slack_x_L_ = CalculateSafeSlack(result, x_bound, x, ip_data_->curr()->z_L());
      }
      trial_slack_x_L_cache_.AddCachedResult1Dep(result, *x);
   }
   return ConstPtr(result);
}

SmartPtr<const Vector> IpoptCalculatedQuantities::trial_slack_x_U()
{
   DBG_START_METH("IpoptCalculatedQuantities::trial_slack_x_U()",
                  dbg_verbosity);

   num_adjusted_slack_x_U_ = 0;
   SmartPtr<Vector> result;
   SmartPtr<const Vector> x = ip_data_->trial()->x();
   SmartPtr<const Vector> x_bound = ip_nlp_->x_U();
   if( !trial_slack_x_U_cache_.GetCachedResult1Dep(result, *x) )
   {
      if( !curr_slack_x_U_cache_.GetCachedResult1Dep(result, *x) )
      {
         SmartPtr<const Matrix> P = ip_nlp_->Px_U();
         result = CalcSlack_U(*P, *x, *x_bound);
         DBG_ASSERT(num_adjusted_slack_x_U_ == 0);
         num_adjusted_slack_x_U_ = CalculateSafeSlack(result, x_bound, x, ip_data_->curr()->z_U());
      }
      trial_slack_x_U_cache_.AddCachedResult1Dep(result, *x);
   }
   return ConstPtr(result);
}

SmartPtr<const Vector> IpoptCalculatedQuantities::trial_slack_s_L()
{
   DBG_START_METH("IpoptCalculatedQuantities::trial_slack_s_L()",
                  dbg_verbosity);

   num_adjusted_slack_s_L_ = 0;
   SmartPtr<Vector> result;
   SmartPtr<const Vector> s = ip_data_->trial()->s();
   SmartPtr<const Vector> s_bound = ip_nlp_->d_L();
   if( !trial_slack_s_L_cache_.GetCachedResult1Dep(result, *s) )
   {
      if( !curr_slack_s_L_cache_.GetCachedResult1Dep(result, *s) )
      {
         SmartPtr<const Matrix> P = ip_nlp_->Pd_L();
         result = CalcSlack_L(*P, *s, *s_bound);
         DBG_ASSERT(num_adjusted_slack_s_L_ == 0);
         num_adjusted_slack_s_L_ = CalculateSafeSlack(result, s_bound, s, ip_data_->curr()->v_L());
      }
      trial_slack_s_L_cache_.AddCachedResult1Dep(result, *s);
   }
   return ConstPtr(result);
}

SmartPtr<const Vector> IpoptCalculatedQuantities::trial_slack_s_U()
{
   DBG_START_METH("IpoptCalculatedQuantities::trial_slack_s_U()",
                  dbg_verbosity);

   num_adjusted_slack_s_U_ = 0;
   SmartPtr<Vector> result;
   SmartPtr<const Vector> s = ip_data_->trial()->s();
   SmartPtr<const Vector> s_bound = ip_nlp_->d_U();
   if( !trial_slack_s_U_cache_.GetCachedResult1Dep(result, *s) )
   {
      if( !curr_slack_s_U_cache_.GetCachedResult1Dep(result, *s) )
      {
         SmartPtr<const Matrix> P = ip_nlp_->Pd_U();
         DBG_PRINT_VECTOR(2, "d_U", *s_bound);
         DBG_PRINT_VECTOR(2, "s", *s);
         result = CalcSlack_U(*P, *s, *s_bound);
         DBG_PRINT_VECTOR(2, "result", *result);
         DBG_ASSERT(num_adjusted_slack_s_U_ == 0);
         num_adjusted_slack_s_U_ = CalculateSafeSlack(result, s_bound, s, ip_data_->curr()->v_U());
         DBG_PRINT((1, "num_adjusted_slack_s_U = %d\n", num_adjusted_slack_s_U_));
         DBG_PRINT_VECTOR(2, "trial_slack_s_U", *result);
      }
      trial_slack_s_U_cache_.AddCachedResult1Dep(result, *s);
   }
   return ConstPtr(result);
}

Index IpoptCalculatedQuantities::CalculateSafeSlack(
   SmartPtr<Vector>&             slack,
   const SmartPtr<const Vector>& bound,
   const SmartPtr<const Vector>& /*curr_point*/,
   const SmartPtr<const Vector>& multiplier
)
{
   DBG_START_METH("IpoptCalculatedQuantities::CalculateSafeSlack", dbg_verbosity);
   DBG_ASSERT(initialize_called_);
   Index retval = 0;
   if( slack->Dim() > 0 )
   {
      Number min_slack = slack->Min();
      // TODO we need to make sure that this also works for non-monotone MUs
      Number s_min = std::numeric_limits<Number>::epsilon() * Min(1., ip_data_->curr_mu());
      DBG_PRINT((1, "s_min = %g, min_slack=%g\n", s_min, min_slack));
      if( min_slack < s_min )
      {
         // Need to correct the slacks and calculate new bounds...
         SmartPtr<Vector> t = slack->MakeNew();
         t->Copy(*slack);
         t->AddScalar(-s_min);
         t->ElementWiseSgn();

         SmartPtr<Vector> zero_vec = t->MakeNew();
         zero_vec->Set(0.0);
         t->ElementWiseMin(*zero_vec);
         t->Scal(-1.0);
         retval = (Index) t->Asum();
         DBG_PRINT((1, "Number of slack corrections = %d\n", retval));
         DBG_PRINT_VECTOR(2, "t(sgn)", *t);

         // ToDo AW: I added the follwing line b/c I found a case where
         // slack was negative and this correction produced 0
         slack->ElementWiseMax(*zero_vec);

         SmartPtr<Vector> t2 = t->MakeNew();
         t2->Set(ip_data_->curr_mu());
         t2->ElementWiseDivide(*multiplier);

         SmartPtr<Vector> s_min_vec = t2->MakeNew();
         s_min_vec->Set(s_min);

         t2->ElementWiseMax(*s_min_vec);
         t2->Axpy(-1.0, *slack);
         DBG_PRINT_VECTOR(2, "tw(smin,mu/mult)", *t2);

         t->ElementWiseMultiply(*t2);
         t->Axpy(1.0, *slack);

         SmartPtr<Vector> t_max = t2;
         t_max->Set(1.0);
         SmartPtr<Vector> abs_bound = bound->MakeNew();
         abs_bound->Copy(*bound);
         abs_bound->ElementWiseAbs();
         t_max->ElementWiseMax(*abs_bound);
         DBG_PRINT_VECTOR(2, "t_max1", *t_max);
         DBG_PRINT_VECTOR(2, "slack", *slack);
         t_max->AddOneVector(1.0, *slack, slack_move_);
         DBG_PRINT_VECTOR(2, "t_max2", *t_max);

         t->ElementWiseMin(*t_max);
         DBG_PRINT_VECTOR(2, "new_slack", *t);

         slack = t;
         return retval;
      }
   }

   return retval;
}

Index IpoptCalculatedQuantities::AdjustedTrialSlacks()
{
   DBG_START_METH("IpoptCalculatedQuantities::AdjustedTrialSlacks()",
                  dbg_verbosity);
   Index result =
      (num_adjusted_slack_x_L_ + num_adjusted_slack_x_U_ + num_adjusted_slack_s_L_ + num_adjusted_slack_s_U_);
   DBG_PRINT((1, "result = %d\n", result));
   return result;
}

void IpoptCalculatedQuantities::ResetAdjustedTrialSlacks()
{
   DBG_START_METH("IpoptCalculatedQuantities::ResetAdjustedTrialSlacks()",
                  dbg_verbosity);
   num_adjusted_slack_x_L_ = num_adjusted_slack_x_U_ = num_adjusted_slack_s_L_ = num_adjusted_slack_s_U_ = 0;
}

///////////////////////////////////////////////////////////////////////////
//                          Objective Function                           //
///////////////////////////////////////////////////////////////////////////

Number IpoptCalculatedQuantities::curr_f()
{
   DBG_START_METH("IpoptCalculatedQuantities::curr_f()",
                  dbg_verbosity);
   Number result;
   SmartPtr<const Vector> x = ip_data_->curr()->x();
   DBG_PRINT_VECTOR(2, "curr_x", *x);
   DBG_PRINT((1, "curr_x tag = %u\n", x->GetTag()));

   bool objective_depends_on_mu = ip_nlp_->objective_depends_on_mu();
   std::vector<const TaggedObject*> tdeps(1);
   tdeps[0] = GetRawPtr(x);
   std::vector<Number> sdeps(1);
   if( objective_depends_on_mu )
   {
      sdeps[0] = ip_data_->curr_mu();
   }
   else
   {
      sdeps[0] = -1.;
   }

   if( !curr_f_cache_.GetCachedResult(result, tdeps, sdeps) )
   {
      if( !trial_f_cache_.GetCachedResult(result, tdeps, sdeps) )
      {
         DBG_PRINT((2, "evaluate curr f\n"));
         if( objective_depends_on_mu )
         {
            result = ip_nlp_->f(*x, ip_data_->curr_mu());
         }
         else
         {
            result = ip_nlp_->f(*x);
         }
      }
      curr_f_cache_.AddCachedResult(result, tdeps, sdeps);
   }
   DBG_PRINT((1, "result (curr_f) = %e\n", result));
   return result;
}

Number IpoptCalculatedQuantities::unscaled_curr_f()
{
   return ip_nlp_->NLP_scaling()->unapply_obj_scaling(curr_f());
}

Number IpoptCalculatedQuantities::trial_f()
{
   DBG_START_METH("IpoptCalculatedQuantities::trial_f()",
                  dbg_verbosity);
   Number result;
   SmartPtr<const Vector> x = ip_data_->trial()->x();
   DBG_PRINT_VECTOR(2, "trial_x", *x);
   DBG_PRINT((1, "trial_x tag = %u\n", x->GetTag()));

   bool objective_depends_on_mu = ip_nlp_->objective_depends_on_mu();
   std::vector<const TaggedObject*> tdeps(1);
   tdeps[0] = GetRawPtr(x);
   std::vector<Number> sdeps(1);
   if( objective_depends_on_mu )
   {
      sdeps[0] = ip_data_->curr_mu();
   }
   else
   {
      sdeps[0] = -1.;
   }

   if( !trial_f_cache_.GetCachedResult(result, tdeps, sdeps) )
   {
      if( !curr_f_cache_.GetCachedResult(result, tdeps, sdeps) )
      {
         DBG_PRINT((2, "evaluate trial f\n"));
         if( objective_depends_on_mu )
         {
            result = ip_nlp_->f(*x, ip_data_->curr_mu());
         }
         else
         {
            result = ip_nlp_->f(*x);
         }
      }
      trial_f_cache_.AddCachedResult(result, tdeps, sdeps);
   }
   DBG_PRINT((1, "result (trial_f) = %e\n", result));
   return result;
}

Number IpoptCalculatedQuantities::unscaled_trial_f()
{
   return ip_nlp_->NLP_scaling()->unapply_obj_scaling(trial_f());
}

SmartPtr<const Vector> IpoptCalculatedQuantities::curr_grad_f()
{
   DBG_START_METH("IpoptCalculatedQuantities::curr_grad_f()",
                  dbg_verbosity);
   SmartPtr<const Vector> result;
   SmartPtr<const Vector> x = ip_data_->curr()->x();

   bool objective_depends_on_mu = ip_nlp_->objective_depends_on_mu();
   std::vector<const TaggedObject*> tdeps(1);
   tdeps[0] = GetRawPtr(x);
   std::vector<Number> sdeps(1);
   if( objective_depends_on_mu )
   {
      sdeps[0] = ip_data_->curr_mu();
   }
   else
   {
      sdeps[0] = -1.;
   }

   if( !curr_grad_f_cache_.GetCachedResult(result, tdeps, sdeps) )
   {
      if( !trial_grad_f_cache_.GetCachedResult(result, tdeps, sdeps) )
      {
         if( objective_depends_on_mu )
         {
            result = ip_nlp_->grad_f(*x, ip_data_->curr_mu());
         }
         else
         {
            result = ip_nlp_->grad_f(*x);
         }
      }
      curr_grad_f_cache_.AddCachedResult(result, tdeps, sdeps);
   }
   return result;
}

SmartPtr<const Vector> IpoptCalculatedQuantities::trial_grad_f()
{
   DBG_START_METH("IpoptCalculatedQuantities::trial_grad_f()",
                  dbg_verbosity);
   SmartPtr<const Vector> result;
   SmartPtr<const Vector> x = ip_data_->trial()->x();

   bool objective_depends_on_mu = ip_nlp_->objective_depends_on_mu();
   std::vector<const TaggedObject*> tdeps(1);
   tdeps[0] = GetRawPtr(x);
   std::vector<Number> sdeps(1);
   if( objective_depends_on_mu )
   {
      sdeps[0] = ip_data_->curr_mu();
   }
   else
   {
      sdeps[0] = -1.;
   }

   if( !trial_grad_f_cache_.GetCachedResult(result, tdeps, sdeps) )
   {
      if( !curr_grad_f_cache_.GetCachedResult(result, tdeps, sdeps) )
      {
         if( objective_depends_on_mu )
         {
            result = ip_nlp_->grad_f(*x, ip_data_->curr_mu());
         }
         else
         {
            result = ip_nlp_->grad_f(*x);
         }
      }
      trial_grad_f_cache_.AddCachedResult(result, tdeps, sdeps);
   }
   return result;
}

///////////////////////////////////////////////////////////////////////////
//                    Barrier Objective Function                         //
///////////////////////////////////////////////////////////////////////////
Number IpoptCalculatedQuantities::CalcBarrierTerm(
   Number        mu,
   const Vector& slack_x_L,
   const Vector& slack_x_U,
   const Vector& slack_s_L,
   const Vector& slack_s_U
)
{
   DBG_START_METH("IpoptCalculatedQuantities::CalcBarrierTerm",
                  dbg_verbosity);
   DBG_ASSERT(initialize_called_);

   DBG_PRINT_VECTOR(2, "slack_x_L", slack_x_L);
   DBG_PRINT_VECTOR(2, "slack_x_U", slack_x_U);
   DBG_PRINT_VECTOR(2, "slack_s_L", slack_s_L);
   DBG_PRINT_VECTOR(2, "slack_s_U", slack_s_U);

   Number retval = 0.;
   retval += slack_x_L.SumLogs();
   DBG_PRINT((1, "BarrierTerm after x_L = %25.16e\n", retval));
   retval += slack_x_U.SumLogs();
   DBG_PRINT((1, "BarrierTerm after x_U = %25.16e\n", retval));
   retval += slack_s_L.SumLogs();
   DBG_PRINT((1, "BarrierTerm after s_L = %25.16e\n", retval));
   retval += slack_s_U.SumLogs();
   DBG_PRINT((1, "BarrierTerm after s_U = %25.16e\n", retval));
   retval *= -mu;

   DBG_PRINT((1, "BarrierTerm without damping = %25.16e\n", retval));

   // Include the linear damping term if kappa_d is nonzero.
   if( kappa_d_ > 0 )
   {
      SmartPtr<const Vector> dampind_x_L;
      SmartPtr<const Vector> dampind_x_U;
      SmartPtr<const Vector> dampind_s_L;
      SmartPtr<const Vector> dampind_s_U;
      ComputeDampingIndicators(dampind_x_L, dampind_x_U, dampind_s_L, dampind_s_U);

      Tmp_x_L().Copy(slack_x_L);
      Tmp_x_L().ElementWiseMultiply(*dampind_x_L);
      retval += kappa_d_ * mu * Tmp_x_L().Asum();
      Tmp_x_U().Copy(slack_x_U);
      Tmp_x_U().ElementWiseMultiply(*dampind_x_U);
      retval += kappa_d_ * mu * Tmp_x_U().Asum();
      Tmp_s_L().Copy(slack_s_L);
      Tmp_s_L().ElementWiseMultiply(*dampind_s_L);
      retval += kappa_d_ * mu * Tmp_s_L().Asum();
      Tmp_s_U().Copy(slack_s_U);
      Tmp_s_U().ElementWiseMultiply(*dampind_s_U);
      retval += kappa_d_ * mu * Tmp_s_U().Asum();
   }

   DBG_PRINT((1, "BarrierTerm with damping = %25.16e\n", retval));

   DBG_ASSERT(IsFiniteNumber(retval));
   return retval;
}

Number IpoptCalculatedQuantities::curr_barrier_obj()
{
   DBG_START_METH("IpoptCalculatedQuantities::curr_barrier_obj()",
                  dbg_verbosity);
   Number result;

   SmartPtr<const Vector> x = ip_data_->curr()->x();
   SmartPtr<const Vector> s = ip_data_->curr()->s();
   DBG_PRINT_VECTOR(2, "curr_x", *x);
   DBG_PRINT_VECTOR(2, "curr_s", *s);
   std::vector<const TaggedObject*> tdeps(2);
   tdeps[0] = GetRawPtr(x);
   tdeps[1] = GetRawPtr(s);

   Number mu = ip_data_->curr_mu();
   DBG_PRINT((1, "curr_mu=%e\n", mu));
   std::vector<Number> sdeps(1);
   sdeps[0] = mu;

   if( !curr_barrier_obj_cache_.GetCachedResult(result, tdeps, sdeps) )
   {
      if( !trial_barrier_obj_cache_.GetCachedResult(result, tdeps, sdeps) )
      {
         result = curr_f();
         DBG_PRINT((1, "curr_F=%e\n", result));
         result += CalcBarrierTerm(mu, *curr_slack_x_L(), *curr_slack_x_U(), *curr_slack_s_L(), *curr_slack_s_U());
      }
      curr_barrier_obj_cache_.AddCachedResult(result, tdeps, sdeps);
   }
   DBG_ASSERT(IsFiniteNumber(result));
   return result;
}

Number IpoptCalculatedQuantities::trial_barrier_obj()
{
   DBG_START_METH("IpoptCalculatedQuantities::trial_barrier_obj()",
                  dbg_verbosity);
   Number result;

   SmartPtr<const Vector> x = ip_data_->trial()->x();
   SmartPtr<const Vector> s = ip_data_->trial()->s();
   DBG_PRINT_VECTOR(2, "trial_x", *x);
   DBG_PRINT_VECTOR(2, "trial_s", *s);
   std::vector<const TaggedObject*> tdeps(2);
   tdeps[0] = GetRawPtr(x);
   tdeps[1] = GetRawPtr(s);

   Number mu = ip_data_->curr_mu();
   DBG_PRINT((1, "trial_mu=%e\n", mu));
   std::vector<Number> sdeps(1);
   sdeps[0] = mu;

   if( !trial_barrier_obj_cache_.GetCachedResult(result, tdeps, sdeps) )
   {
      if( !curr_barrier_obj_cache_.GetCachedResult(result, tdeps, sdeps) )
      {
         result = trial_f();
         DBG_PRINT((1, "trial_F=%e\n", result));
         DBG_PRINT_VECTOR(2, "trial_slack_s_U", *trial_slack_s_U());
         result += CalcBarrierTerm(ip_data_->curr_mu(), *trial_slack_x_L(), *trial_slack_x_U(), *trial_slack_s_L(),
                                   *trial_slack_s_U());
      }
      trial_barrier_obj_cache_.AddCachedResult(result, tdeps, sdeps);
   }
   DBG_ASSERT(IsFiniteNumber(result));
   return result;
}

SmartPtr<const Vector> IpoptCalculatedQuantities::curr_grad_barrier_obj_x()
{
   DBG_START_METH("IpoptCalculatedQuantities::curr_grad_barrier_obj_x()",
                  dbg_verbosity);
   DBG_ASSERT(initialize_called_);
   SmartPtr<const Vector> result;

   SmartPtr<const Vector> x = ip_data_->curr()->x();
   std::vector<const TaggedObject*> tdeps(1);
   tdeps[0] = GetRawPtr(x);
   Number mu = ip_data_->curr_mu();
   std::vector<Number> sdeps(1);
   sdeps[0] = mu;
   DBG_PRINT((1, "curr_mu=%e\n", mu));

   if( !curr_grad_barrier_obj_x_cache_.GetCachedResult(result, tdeps, sdeps) )
   {
      SmartPtr<Vector> tmp1 = x->MakeNew();
      tmp1->Copy(*curr_grad_f());

      Tmp_x_L().Set(1.);
      ip_nlp_->Px_L()->AddMSinvZ(-mu, *curr_slack_x_L(), Tmp_x_L(), *tmp1);

      Tmp_x_U().Set(1.);
      ip_nlp_->Px_U()->AddMSinvZ(mu, *curr_slack_x_U(), Tmp_x_U(), *tmp1);

      DBG_PRINT_VECTOR(2, "Barrier_Grad_x without damping", *tmp1);

      // Take care of linear damping terms
      if( kappa_d_ > 0. )
      {
         SmartPtr<const Vector> dampind_x_L;
         SmartPtr<const Vector> dampind_x_U;
         SmartPtr<const Vector> dampind_s_L;
         SmartPtr<const Vector> dampind_s_U;
         ComputeDampingIndicators(dampind_x_L, dampind_x_U, dampind_s_L, dampind_s_U);

         DBG_PRINT((1, "kappa_d*mu = %e\n", kappa_d_ * mu));
         DBG_PRINT_VECTOR(2, "dampind_x_L", *dampind_x_L);
         ip_nlp_->Px_L()->MultVector(kappa_d_ * mu, *dampind_x_L, 1., *tmp1);
         ip_nlp_->Px_U()->MultVector(-kappa_d_ * mu, *dampind_x_U, 1., *tmp1);
      }

      DBG_PRINT_VECTOR(2, "Barrier_Grad_x with damping", *tmp1);

      result = ConstPtr(tmp1);

      curr_grad_barrier_obj_x_cache_.AddCachedResult(result, tdeps, sdeps);
   }

   return result;
}

SmartPtr<const Vector> IpoptCalculatedQuantities::grad_kappa_times_damping_x()
{
   DBG_START_METH("IpoptCalculatedQuantities::grad_kappa_times_damping_x()",
                  dbg_verbosity);
   DBG_ASSERT(initialize_called_);
   SmartPtr<const Vector> result;
   SmartPtr<const Vector> x = ip_data_->curr()->x();

   std::vector<const TaggedObject*> tdeps(2);
   tdeps[0] = GetRawPtr(ip_nlp_->Px_L());
   tdeps[1] = GetRawPtr(ip_nlp_->Px_U());
   std::vector<Number> sdeps(1);
   sdeps[0] = kappa_d_;
   if( !grad_kappa_times_damping_x_cache_.GetCachedResult(result, tdeps, sdeps) )
   {
      SmartPtr<Vector> tmp1 = x->MakeNew();
      if( kappa_d_ > 0. )
      {
         SmartPtr<const Vector> dampind_x_L;
         SmartPtr<const Vector> dampind_x_U;
         SmartPtr<const Vector> dampind_s_L;
         SmartPtr<const Vector> dampind_s_U;
         ComputeDampingIndicators(dampind_x_L, dampind_x_U, dampind_s_L, dampind_s_U);

         ip_nlp_->Px_L()->MultVector(kappa_d_, *dampind_x_L, 0., *tmp1);
         ip_nlp_->Px_U()->MultVector(-kappa_d_, *dampind_x_U, 1., *tmp1);
      }
      else
      {
         tmp1->Set(0.);
      }
      result = ConstPtr(tmp1);

      grad_kappa_times_damping_x_cache_.AddCachedResult(result, tdeps, sdeps);
   }

   return result;
}

SmartPtr<const Vector> IpoptCalculatedQuantities::curr_grad_barrier_obj_s()
{
   DBG_START_METH("IpoptCalculatedQuantities::curr_grad_barrier_obj_s()",
                  dbg_verbosity);
   DBG_ASSERT(initialize_called_);
   SmartPtr<const Vector> result;

   SmartPtr<const Vector> s = ip_data_->curr()->s();
   std::vector<const TaggedObject*> tdeps(1);
   tdeps[0] = GetRawPtr(s);
   Number mu = ip_data_->curr_mu();
   std::vector<Number> sdeps(1);
   sdeps[0] = mu;
   DBG_PRINT((1, "curr_mu=%e\n", mu));

   if( !curr_grad_barrier_obj_s_cache_.GetCachedResult(result, tdeps, sdeps) )
   {
      SmartPtr<Vector> tmp1 = s->MakeNew();

      Tmp_s_L().Set(-mu);
      Tmp_s_L().ElementWiseDivide(*curr_slack_s_L());
      ip_nlp_->Pd_L()->MultVector(1., Tmp_s_L(), 0., *tmp1);

      Tmp_s_U().Set(1.);
      ip_nlp_->Pd_U()->AddMSinvZ(mu, *curr_slack_s_U(), Tmp_s_U(), *tmp1);

      DBG_PRINT_VECTOR(2, "Barrier_Grad_s without damping", *tmp1);

      // Take care of linear damping terms
      if( kappa_d_ > 0. )
      {
         SmartPtr<const Vector> dampind_x_L;
         SmartPtr<const Vector> dampind_x_U;
         SmartPtr<const Vector> dampind_s_L;
         SmartPtr<const Vector> dampind_s_U;
         ComputeDampingIndicators(dampind_x_L, dampind_x_U, dampind_s_L, dampind_s_U);

         DBG_PRINT((1, "kappa_d*mu = %e\n", kappa_d_ * mu));
         DBG_PRINT_VECTOR(2, "dampind_s_L", *dampind_s_L);
         DBG_PRINT_VECTOR(2, "dampind_s_U", *dampind_s_U);
         ip_nlp_->Pd_L()->MultVector(kappa_d_ * mu, *dampind_s_L, 1., *tmp1);
         ip_nlp_->Pd_U()->MultVector(-kappa_d_ * mu, *dampind_s_U, 1., *tmp1);
      }

      DBG_PRINT_VECTOR(2, "Barrier_Grad_s with damping", *tmp1);

      result = ConstPtr(tmp1);

      curr_grad_barrier_obj_s_cache_.AddCachedResult(result, tdeps, sdeps);
   }

   return result;
}

SmartPtr<const Vector> IpoptCalculatedQuantities::grad_kappa_times_damping_s()
{
   DBG_START_METH("IpoptCalculatedQuantities::grad_kappa_times_damping_s()",
                  dbg_verbosity);
   DBG_ASSERT(initialize_called_);
   SmartPtr<const Vector> result;
   SmartPtr<const Vector> s = ip_data_->curr()->s();

   std::vector<const TaggedObject*> tdeps(2);
   tdeps[0] = GetRawPtr(ip_nlp_->Pd_L());
   tdeps[1] = GetRawPtr(ip_nlp_->Pd_U());
   std::vector<Number> sdeps(1);
   sdeps[0] = kappa_d_;
   if( !grad_kappa_times_damping_s_cache_.GetCachedResult(result, tdeps, sdeps) )
   {
      SmartPtr<Vector> tmp1 = s->MakeNew();
      if( kappa_d_ > 0. )
      {
         SmartPtr<const Vector> dampind_x_L;
         SmartPtr<const Vector> dampind_x_U;
         SmartPtr<const Vector> dampind_s_L;
         SmartPtr<const Vector> dampind_s_U;
         ComputeDampingIndicators(dampind_x_L, dampind_x_U, dampind_s_L, dampind_s_U);

         ip_nlp_->Pd_L()->MultVector(kappa_d_, *dampind_s_L, 0., *tmp1);
         ip_nlp_->Pd_U()->MultVector(-kappa_d_, *dampind_s_U, 1., *tmp1);
      }
      else
      {
         tmp1->Set(0.);
      }
      result = ConstPtr(tmp1);

      grad_kappa_times_damping_s_cache_.AddCachedResult(result, tdeps, sdeps);
   }

   return result;
}

void IpoptCalculatedQuantities::ComputeDampingIndicators(
   SmartPtr<const Vector>& dampind_x_L,
   SmartPtr<const Vector>& dampind_x_U,
   SmartPtr<const Vector>& dampind_s_L,
   SmartPtr<const Vector>& dampind_s_U
)
{
   DBG_START_METH("IpoptCalculatedQuantities::ComputeDampingFilters()",
                  dbg_verbosity);

   // Assume that all indicators have to be computed if one of the
   // SmartPtrs is still zero.
   if( IsNull(dampind_x_L_) )
   {
      // First for x
      Tmp_x_L().Set(1.0);
      ip_nlp_->Px_L()->MultVector(1.0, Tmp_x_L(), 0.0, Tmp_x());
      Tmp_x_U().Set(1.0);
      ip_nlp_->Px_U()->MultVector(-1.0, Tmp_x_U(), 1.0, Tmp_x());

      dampind_x_L_ = ip_nlp_->x_L()->MakeNew();
      ip_nlp_->Px_L()->TransMultVector(1.0, Tmp_x(), 0.0, *dampind_x_L_);

      dampind_x_U_ = ip_nlp_->x_U()->MakeNew();
      ip_nlp_->Px_U()->TransMultVector(-1.0, Tmp_x(), 0.0, *dampind_x_U_);

      // Now for s
      Tmp_s_L().Set(1.0);
      ip_nlp_->Pd_L()->MultVector(1.0, Tmp_s_L(), 0.0, Tmp_s());
      Tmp_s_U().Set(1.0);
      ip_nlp_->Pd_U()->MultVector(-1.0, Tmp_s_U(), 1.0, Tmp_s());

      dampind_s_L_ = ip_nlp_->d_L()->MakeNew();
      ip_nlp_->Pd_L()->TransMultVector(1.0, Tmp_s(), 0.0, *dampind_s_L_);

      dampind_s_U_ = ip_nlp_->d_U()->MakeNew();
      ip_nlp_->Pd_U()->TransMultVector(-1.0, Tmp_s(), 0.0, *dampind_s_U_);

      DBG_PRINT_VECTOR(2, "dampind_x_L_", *dampind_x_L_);
      DBG_PRINT_VECTOR(2, "dampind_x_U_", *dampind_x_U_);
      DBG_PRINT_VECTOR(2, "dampind_s_L_", *dampind_s_L_);
      DBG_PRINT_VECTOR(2, "dampind_s_U_", *dampind_s_U_);
   }

   dampind_x_L = ConstPtr(dampind_x_L_);
   dampind_x_U = ConstPtr(dampind_x_U_);
   dampind_s_L = ConstPtr(dampind_s_L_);
   dampind_s_U = ConstPtr(dampind_s_U_);
}

///////////////////////////////////////////////////////////////////////////
//                                Constraints                            //
///////////////////////////////////////////////////////////////////////////

SmartPtr<const Vector> IpoptCalculatedQuantities::curr_c()
{
   DBG_START_METH("IpoptCalculatedQuantities::curr_c()",
                  dbg_verbosity);
   SmartPtr<const Vector> result;
   SmartPtr<const Vector> x = ip_data_->curr()->x();

   if( !curr_c_cache_.GetCachedResult1Dep(result, *x) )
   {
      if( !trial_c_cache_.GetCachedResult1Dep(result, *x) )
      {
         result = ip_nlp_->c(*x);
      }
      curr_c_cache_.AddCachedResult1Dep(result, *x);
   }
   return result;
}

SmartPtr<const Vector> IpoptCalculatedQuantities::unscaled_curr_c()
{
   return ip_nlp_->NLP_scaling()->unapply_vector_scaling_c(curr_c());
}

SmartPtr<const Vector> IpoptCalculatedQuantities::trial_c()
{
   DBG_START_METH("IpoptCalculatedQuantities::trial_c()",
                  dbg_verbosity);
   SmartPtr<const Vector> result;
   SmartPtr<const Vector> x = ip_data_->trial()->x();

   if( !trial_c_cache_.GetCachedResult1Dep(result, *x) )
   {
      if( !curr_c_cache_.GetCachedResult1Dep(result, *x) )
      {
         result = ip_nlp_->c(*x);
      }
      trial_c_cache_.AddCachedResult1Dep(result, *x);
   }
   return result;
}

SmartPtr<const Vector> IpoptCalculatedQuantities::unscaled_trial_c()
{
   return ip_nlp_->NLP_scaling()->unapply_vector_scaling_c(trial_c());
}

SmartPtr<const Vector> IpoptCalculatedQuantities::curr_d()
{
   DBG_START_METH("IpoptCalculatedQuantities::curr_d()",
                  dbg_verbosity);
   SmartPtr<const Vector> result;
   SmartPtr<const Vector> x = ip_data_->curr()->x();

   if( !curr_d_cache_.GetCachedResult1Dep(result, *x) )
   {
      if( !trial_d_cache_.GetCachedResult1Dep(result, *x) )
      {
         result = ip_nlp_->d(*x);
      }
      curr_d_cache_.AddCachedResult1Dep(result, *x);
   }
   return result;
}

SmartPtr<const Vector> IpoptCalculatedQuantities::unscaled_curr_d()
{
   return ip_nlp_->NLP_scaling()->unapply_vector_scaling_d(curr_d());
}

SmartPtr<const Vector> IpoptCalculatedQuantities::trial_d()
{
   DBG_START_METH("IpoptCalculatedQuantities::trial_d()",
                  dbg_verbosity);
   SmartPtr<const Vector> result;
   SmartPtr<const Vector> x = ip_data_->trial()->x();

   if( !trial_d_cache_.GetCachedResult1Dep(result, *x) )
   {
      if( !curr_d_cache_.GetCachedResult1Dep(result, *x) )
      {
         result = ip_nlp_->d(*x);
      }
      trial_d_cache_.AddCachedResult1Dep(result, *x);
   }
   return result;
}

SmartPtr<const Vector> IpoptCalculatedQuantities::curr_d_minus_s()
{
   DBG_START_METH("IpoptCalculatedQuantities::curr_d_minus_s()",
                  dbg_verbosity);
   SmartPtr<const Vector> result;

   SmartPtr<const Vector> x = ip_data_->curr()->x();
   SmartPtr<const Vector> s = ip_data_->curr()->s();

   if( !curr_d_minus_s_cache_.GetCachedResult2Dep(result, *x, *s) )
   {
      if( !trial_d_minus_s_cache_.GetCachedResult2Dep(result, *x, *s) )
      {
         SmartPtr<Vector> tmp = s->MakeNew();
         tmp->AddTwoVectors(1., *curr_d(), -1., *s, 0.);
         result = ConstPtr(tmp);
      }
      curr_d_minus_s_cache_.AddCachedResult2Dep(result, *x, *s);
   }

   return result;
}

SmartPtr<const Vector> IpoptCalculatedQuantities::trial_d_minus_s()
{
   DBG_START_METH("IpoptCalculatedQuantities::trial_d_minus_s()",
                  dbg_verbosity);
   SmartPtr<const Vector> result;

   SmartPtr<const Vector> x = ip_data_->trial()->x();
   SmartPtr<const Vector> s = ip_data_->trial()->s();

   if( !trial_d_minus_s_cache_.GetCachedResult2Dep(result, *x, *s) )
   {
      if( !curr_d_minus_s_cache_.GetCachedResult2Dep(result, *x, *s) )
      {
         SmartPtr<Vector> tmp = s->MakeNew();
         tmp->AddTwoVectors(1., *trial_d(), -1., *s, 0.);
         result = ConstPtr(tmp);
      }
      trial_d_minus_s_cache_.AddCachedResult2Dep(result, *x, *s);
   }

   return result;
}

SmartPtr<const Matrix> IpoptCalculatedQuantities::curr_jac_c()
{
   DBG_START_METH("IpoptCalculatedQuantities::curr_jac_c()",
                  dbg_verbosity);
   SmartPtr<const Matrix> result;
   SmartPtr<const Vector> x = ip_data_->curr()->x();

   if( !curr_jac_c_cache_.GetCachedResult1Dep(result, *x) )
   {
      if( !trial_jac_c_cache_.GetCachedResult1Dep(result, *x) )
      {
         result = ip_nlp_->jac_c(*x);
      }
      curr_jac_c_cache_.AddCachedResult1Dep(result, *x);
   }
   return result;
}

SmartPtr<const Matrix> IpoptCalculatedQuantities::trial_jac_c()
{
   DBG_START_METH("IpoptCalculatedQuantities::trial_jac_c()",
                  dbg_verbosity);
   SmartPtr<const Matrix> result;
   SmartPtr<const Vector> x = ip_data_->trial()->x();

   if( !trial_jac_c_cache_.GetCachedResult1Dep(result, *x) )
   {
      if( !curr_jac_c_cache_.GetCachedResult1Dep(result, *x) )
      {
         result = ip_nlp_->jac_c(*x);
      }
      trial_jac_c_cache_.AddCachedResult1Dep(result, *x);
   }
   return result;
}

SmartPtr<const Matrix> IpoptCalculatedQuantities::curr_jac_d()
{
   DBG_START_METH("IpoptCalculatedQuantities::curr_jac_d()",
                  dbg_verbosity);
   SmartPtr<const Matrix> result;
   SmartPtr<const Vector> x = ip_data_->curr()->x();

   if( !curr_jac_d_cache_.GetCachedResult1Dep(result, *x) )
   {
      if( !trial_jac_d_cache_.GetCachedResult1Dep(result, *x) )
      {
         result = ip_nlp_->jac_d(*x);
      }
      curr_jac_d_cache_.AddCachedResult1Dep(result, *x);
   }
   return result;
}

SmartPtr<const Matrix> IpoptCalculatedQuantities::trial_jac_d()
{
   DBG_START_METH("IpoptCalculatedQuantities::trial_jac_d()",
                  dbg_verbosity);
   SmartPtr<const Matrix> result;
   SmartPtr<const Vector> x = ip_data_->trial()->x();

   if( !trial_jac_d_cache_.GetCachedResult1Dep(result, *x) )
   {
      if( !curr_jac_d_cache_.GetCachedResult1Dep(result, *x) )
      {
         result = ip_nlp_->jac_d(*x);
      }
      trial_jac_d_cache_.AddCachedResult1Dep(result, *x);
   }
   return result;
}

SmartPtr<const Vector> IpoptCalculatedQuantities::curr_jac_c_times_vec(
   const Vector& vec
)
{
   DBG_START_METH("IpoptCalculatedQuantities::curr_jac_c_times_vec",
                  dbg_verbosity);
   SmartPtr<const Vector> result;
   SmartPtr<const Vector> x = ip_data_->curr()->x();

   if( !curr_jac_c_times_vec_cache_.GetCachedResult2Dep(result, *x, vec) )
   {
      SmartPtr<Vector> tmp = ip_data_->curr()->y_c()->MakeNew();
      curr_jac_c()->MultVector(1.0, vec, 0., *tmp);
      result = ConstPtr(tmp);
      curr_jac_c_times_vec_cache_.AddCachedResult2Dep(result, *x, vec);
   }

   return result;
}

SmartPtr<const Vector> IpoptCalculatedQuantities::curr_jac_d_times_vec(
   const Vector& vec
)
{
   DBG_START_METH("IpoptCalculatedQuantities::curr_jac_d_times_vec()",
                  dbg_verbosity);
   SmartPtr<const Vector> result;
   SmartPtr<const Vector> x = ip_data_->curr()->x();

   if( !curr_jac_d_times_vec_cache_.GetCachedResult2Dep(result, *x, vec) )
   {
      SmartPtr<Vector> tmp = ip_data_->curr()->s()->MakeNew();
      DBG_PRINT_VECTOR(1, "vec", vec);
      DBG_PRINT_MATRIX(2, "curr_jac_d()", *curr_jac_d());
      curr_jac_d()->MultVector(1.0, vec, 0., *tmp);
      result = ConstPtr(tmp);
      curr_jac_d_times_vec_cache_.AddCachedResult2Dep(result, *x, vec);
   }

   return result;
}

SmartPtr<const Vector> IpoptCalculatedQuantities::curr_jac_cT_times_curr_y_c()
{
   DBG_START_METH("IpoptCalculatedQuantities::curr_jac_cT_times_curr_y_c()",
                  dbg_verbosity);
   return curr_jac_cT_times_vec(*ip_data_->curr()->y_c());
}

SmartPtr<const Vector> IpoptCalculatedQuantities::trial_jac_cT_times_trial_y_c()
{
   DBG_START_METH("IpoptCalculatedQuantities::trial_jac_cT_times_trial_y_c()",
                  dbg_verbosity);
   return trial_jac_cT_times_vec(*ip_data_->trial()->y_c());
}

SmartPtr<const Vector> IpoptCalculatedQuantities::curr_jac_dT_times_curr_y_d()
{
   DBG_START_METH("IpoptCalculatedQuantities::curr_jac_dT_times_curr_y_d()",
                  dbg_verbosity);
   return curr_jac_dT_times_vec(*ip_data_->curr()->y_d());
}

SmartPtr<const Vector> IpoptCalculatedQuantities::trial_jac_dT_times_trial_y_d()
{
   DBG_START_METH("IpoptCalculatedQuantities::trial_jac_dT_times_trial_y_d()",
                  dbg_verbosity);
   return trial_jac_dT_times_vec(*ip_data_->trial()->y_d());
}

SmartPtr<const Vector> IpoptCalculatedQuantities::curr_jac_cT_times_vec(
   const Vector& vec
)
{
   DBG_START_METH("IpoptCalculatedQuantities::curr_jac_cT_times_vec",
                  dbg_verbosity);
   SmartPtr<const Vector> result;
   SmartPtr<const Vector> x = ip_data_->curr()->x();

   if( !curr_jac_cT_times_vec_cache_.GetCachedResult2Dep(result, *x, vec) )
   {
      if( !trial_jac_cT_times_vec_cache_.GetCachedResult2Dep(result, *x, vec) )
      {
         SmartPtr<Vector> tmp = x->MakeNew();
         curr_jac_c()->TransMultVector(1.0, vec, 0., *tmp);
         result = ConstPtr(tmp);
      }
      curr_jac_cT_times_vec_cache_.AddCachedResult2Dep(result, *x, vec);
   }

   return result;
}

SmartPtr<const Vector> IpoptCalculatedQuantities::trial_jac_cT_times_vec(
   const Vector& vec
)
{
   DBG_START_METH("IpoptCalculatedQuantities::trial_jac_cT_times_vec",
                  dbg_verbosity);
   SmartPtr<const Vector> result;
   SmartPtr<const Vector> x = ip_data_->trial()->x();

   if( !trial_jac_cT_times_vec_cache_.GetCachedResult2Dep(result, *x, vec) )
   {
      if( !curr_jac_cT_times_vec_cache_.GetCachedResult2Dep(result, *x, vec) )
      {
         SmartPtr<Vector> tmp = x->MakeNew();
         trial_jac_c()->TransMultVector(1.0, vec, 0., *tmp);
         result = ConstPtr(tmp);
      }
      trial_jac_cT_times_vec_cache_.AddCachedResult2Dep(result, *x, vec);
   }

   return result;
}

SmartPtr<const Vector> IpoptCalculatedQuantities::curr_jac_dT_times_vec(
   const Vector& vec
)
{
   DBG_START_METH("IpoptCalculatedQuantities::curr_jac_dT_times_vec()",
                  dbg_verbosity);
   SmartPtr<const Vector> result;
   SmartPtr<const Vector> x = ip_data_->curr()->x();

   if( !curr_jac_dT_times_vec_cache_.GetCachedResult2Dep(result, *x, vec) )
   {
      if( !trial_jac_dT_times_vec_cache_.GetCachedResult2Dep(result, *x, vec) )
      {
         SmartPtr<Vector> tmp = x->MakeNew();
         DBG_PRINT_VECTOR(2, "vec", vec);
         DBG_PRINT_VECTOR(2, "tmp", *tmp);
         DBG_PRINT_MATRIX(2, "curr_jac_d()", *curr_jac_d());
         curr_jac_d()->TransMultVector(1.0, vec, 0., *tmp);
         result = ConstPtr(tmp);
      }
      curr_jac_dT_times_vec_cache_.AddCachedResult2Dep(result, *x, vec);
   }

   return result;
}

SmartPtr<const Vector> IpoptCalculatedQuantities::trial_jac_dT_times_vec(
   const Vector& vec
)
{
   DBG_START_METH("IpoptCalculatedQuantities::trial_jac_dT_times_vec()",
                  dbg_verbosity);
   SmartPtr<const Vector> result;
   SmartPtr<const Vector> x = ip_data_->trial()->x();

   if( !trial_jac_dT_times_vec_cache_.GetCachedResult2Dep(result, *x, vec) )
   {
      if( !curr_jac_dT_times_vec_cache_.GetCachedResult2Dep(result, *x, vec) )
      {
         SmartPtr<Vector> tmp = x->MakeNew();
         trial_jac_d()->TransMultVector(1.0, vec, 0., *tmp);
         result = ConstPtr(tmp);
      }
      trial_jac_dT_times_vec_cache_.AddCachedResult2Dep(result, *x, vec);
   }

   return result;
}

Number IpoptCalculatedQuantities::curr_constraint_violation()
{
   DBG_START_METH("IpoptCalculatedQuantities::curr_constraint_violation()",
                  dbg_verbosity);
   return curr_primal_infeasibility(constr_viol_normtype_);
}

Number IpoptCalculatedQuantities::trial_constraint_violation()
{
   DBG_START_METH("IpoptCalculatedQuantities::trial_constraint_violation()",
                  dbg_verbosity);
   return trial_primal_infeasibility(constr_viol_normtype_);
}

Number IpoptCalculatedQuantities::curr_nlp_constraint_violation(
   ENormType NormType
)
{
   DBG_START_METH("IpoptCalculatedQuantities::curr_nlp_constraint_violation()",
                  dbg_verbosity);
   Number result;

   SmartPtr<const Vector> x = ip_data_->curr()->x();

   std::vector<const TaggedObject*> deps(1);
   deps[0] = GetRawPtr(x);
   std::vector<Number> sdeps(1);
   sdeps[0] = (Number) NormType;

   if( !curr_nlp_constraint_violation_cache_.GetCachedResult(result, deps, sdeps) )
   {
      SmartPtr<const Vector> c = curr_c();
      SmartPtr<const Vector> d = curr_d();

      SmartPtr<Vector> d_viol_L = ip_nlp_->d_L()->MakeNewCopy();
      ip_nlp_->Pd_L()->TransMultVector(-1., *d, 1., *d_viol_L);
      SmartPtr<Vector> tmp = d_viol_L->MakeNew();
      tmp->Set(0.);
      d_viol_L->ElementWiseMax(*tmp);
      DBG_PRINT_VECTOR(2, "d_viol_L", *d_viol_L);

      SmartPtr<Vector> d_viol_U = ip_nlp_->d_U()->MakeNewCopy();
      ip_nlp_->Pd_U()->TransMultVector(-1., *d, 1., *d_viol_U);
      tmp = d_viol_U->MakeNew();
      tmp->Set(0.);
      d_viol_U->ElementWiseMin(*tmp);
      DBG_PRINT_VECTOR(2, "d_viol_U", *d_viol_U);

      std::vector<SmartPtr<const Vector> > vecs(3);
      vecs[0] = c;
      vecs[1] = GetRawPtr(d_viol_L);
      vecs[2] = GetRawPtr(d_viol_U);
      result = CalcNormOfType(NormType, vecs);
      curr_nlp_constraint_violation_cache_.AddCachedResult(result, deps, sdeps);
   }

   return result;
}

Number IpoptCalculatedQuantities::unscaled_curr_nlp_constraint_violation(
   ENormType NormType
)
{
   DBG_START_METH("IpoptCalculatedQuantities::unscaled_curr_nlp_constraint_violation()",
                  dbg_verbosity);
   Number result;

   SmartPtr<const Vector> x = ip_data_->curr()->x();

   std::vector<const TaggedObject*> deps(1);
   deps[0] = GetRawPtr(x);
   std::vector<Number> sdeps(1);
   sdeps[0] = (Number) NormType;

   if( !unscaled_curr_nlp_constraint_violation_cache_.GetCachedResult(result, deps, sdeps) )
   {
      if( !unscaled_trial_nlp_constraint_violation_cache_.GetCachedResult(result, deps, sdeps) )
      {
         SmartPtr<const Vector> c = unscaled_curr_c();

         SmartPtr<const Vector> d = curr_d();

         SmartPtr<const Vector> d_L = ip_nlp_->d_L();
         SmartPtr<Vector> d_viol_L = d_L->MakeNew();
         if( d_L->Dim() > 0 )
         {
            SmartPtr<Vector> d_viol = d->MakeNewCopy();
            ip_nlp_->Pd_L()->MultVector(1., *d_L, -1., *d_viol);
            SmartPtr<const Vector> d_viol2 = ip_nlp_->NLP_scaling()->unapply_vector_scaling_d(ConstPtr(d_viol));
            ip_nlp_->Pd_L()->TransMultVector(1., *d_viol2, 0., *d_viol_L);
            SmartPtr<Vector> tmp = d_viol_L->MakeNew();
            tmp->Set(0.);
            d_viol_L->ElementWiseMax(*tmp);
         }
         DBG_PRINT_VECTOR(2, "d_viol_L", *d_viol_L);

         SmartPtr<const Vector> d_U = ip_nlp_->d_U();
         SmartPtr<Vector> d_viol_U = d_U->MakeNew();
         if( d_U->Dim() > 0 )
         {
            SmartPtr<Vector> d_viol = d->MakeNewCopy();
            ip_nlp_->Pd_U()->MultVector(1., *d_U, -1., *d_viol);
            SmartPtr<const Vector> d_viol2 = ip_nlp_->NLP_scaling()->unapply_vector_scaling_d(ConstPtr(d_viol));
            ip_nlp_->Pd_U()->TransMultVector(1., *d_viol2, 0., *d_viol_U);
            SmartPtr<Vector> tmp = d_viol_U->MakeNew();
            tmp->Set(0.);
            d_viol_U->ElementWiseMin(*tmp);
         }
         DBG_PRINT_VECTOR(2, "d_viol_U", *d_viol_U);

         std::vector<SmartPtr<const Vector> > vecs(3);
         vecs[0] = c;
         vecs[1] = GetRawPtr(d_viol_L);
         vecs[2] = GetRawPtr(d_viol_U);
         result = CalcNormOfType(NormType, vecs);
      }
      unscaled_curr_nlp_constraint_violation_cache_.AddCachedResult(result, deps, sdeps);
   }

   return result;
}

Number IpoptCalculatedQuantities::unscaled_trial_nlp_constraint_violation(
   ENormType NormType
)
{
   DBG_START_METH("IpoptCalculatedQuantities::unscaled_trial_nlp_constraint_violation()",
                  dbg_verbosity);
   Number result;

   SmartPtr<const Vector> x = ip_data_->trial()->x();

   std::vector<const TaggedObject*> deps(1);
   deps[0] = GetRawPtr(x);
   std::vector<Number> sdeps(1);
   sdeps[0] = (Number) NormType;

   if( !unscaled_trial_nlp_constraint_violation_cache_.GetCachedResult(result, deps, sdeps) )
   {
      if( !unscaled_curr_nlp_constraint_violation_cache_.GetCachedResult(result, deps, sdeps) )
      {
         SmartPtr<const Vector> c = unscaled_trial_c();

         SmartPtr<const Vector> d = trial_d();

         SmartPtr<const Vector> d_L = ip_nlp_->d_L();
         SmartPtr<Vector> d_viol_L = d_L->MakeNew();
         if( d_L->Dim() > 0 )
         {
            SmartPtr<Vector> d_viol = d->MakeNewCopy();
            ip_nlp_->Pd_L()->MultVector(1., *d_L, -1., *d_viol);
            SmartPtr<const Vector> d_viol2 = ip_nlp_->NLP_scaling()->unapply_vector_scaling_d(ConstPtr(d_viol));
            ip_nlp_->Pd_L()->TransMultVector(1., *d_viol2, 0., *d_viol_L);
            SmartPtr<Vector> tmp = d_viol_L->MakeNew();
            tmp->Set(0.);
            d_viol_L->ElementWiseMax(*tmp);
         }
         DBG_PRINT_VECTOR(2, "d_viol_L", *d_viol_L);

         SmartPtr<const Vector> d_U = ip_nlp_->d_U();
         SmartPtr<Vector> d_viol_U = d_U->MakeNew();
         if( d_U->Dim() > 0 )
         {
            SmartPtr<Vector> d_viol = d->MakeNewCopy();
            ip_nlp_->Pd_U()->MultVector(1., *d_U, -1., *d_viol);
            SmartPtr<const Vector> d_viol2 = ip_nlp_->NLP_scaling()->unapply_vector_scaling_d(ConstPtr(d_viol));
            ip_nlp_->Pd_U()->TransMultVector(1., *d_viol2, 0., *d_viol_U);
            SmartPtr<Vector> tmp = d_viol_U->MakeNew();
            tmp->Set(0.);
            d_viol_U->ElementWiseMin(*tmp);
         }
         DBG_PRINT_VECTOR(2, "d_viol_U", *d_viol_U);

         std::vector<SmartPtr<const Vector> > vecs(3);
         vecs[0] = c;
         vecs[1] = GetRawPtr(d_viol_L);
         vecs[2] = GetRawPtr(d_viol_U);
         result = CalcNormOfType(NormType, vecs);
      }
      unscaled_trial_nlp_constraint_violation_cache_.AddCachedResult(result, deps, sdeps);
   }

   return result;
}

///////////////////////////////////////////////////////////////////////////
//                Exact Hessian using second derivatives                 //
///////////////////////////////////////////////////////////////////////////

SmartPtr<const SymMatrix> IpoptCalculatedQuantities::curr_exact_hessian()
{
   DBG_START_METH("IpoptCalculatedQuantities::curr_exact_hessian()",
                  dbg_verbosity);

   SmartPtr<const SymMatrix> result;
   SmartPtr<const Vector> x = ip_data_->curr()->x();
   SmartPtr<const Vector> y_c = ip_data_->curr()->y_c();
   SmartPtr<const Vector> y_d = ip_data_->curr()->y_d();

   bool objective_depends_on_mu = ip_nlp_->objective_depends_on_mu();
   std::vector<const TaggedObject*> tdeps(3);
   tdeps[0] = GetRawPtr(x);
   tdeps[1] = GetRawPtr(y_c);
   tdeps[2] = GetRawPtr(y_d);
   std::vector<Number> sdeps(1);
   if( objective_depends_on_mu )
   {
      sdeps[0] = ip_data_->curr_mu();
   }
   else
   {
      sdeps[0] = -1.;
   }

   if( !curr_exact_hessian_cache_.GetCachedResult(result, tdeps, sdeps) )
   {
      if( objective_depends_on_mu )
      {
         result = ip_nlp_->h(*x, 1.0, *y_c, *y_d, ip_data_->curr_mu());
      }
      else
      {
         result = ip_nlp_->h(*x, 1.0, *y_c, *y_d);
      }
      curr_exact_hessian_cache_.AddCachedResult(result, tdeps, sdeps);
   }

   return result;
}

///////////////////////////////////////////////////////////////////////////
//                  Optimality Error and its components                  //
///////////////////////////////////////////////////////////////////////////

SmartPtr<const Vector> IpoptCalculatedQuantities::curr_grad_lag_x()
{
   DBG_START_METH("IpoptCalculatedQuantities::curr_grad_lag_x()",
                  dbg_verbosity);
   SmartPtr<const Vector> result;

   SmartPtr<const Vector> x = ip_data_->curr()->x();
   SmartPtr<const Vector> y_c = ip_data_->curr()->y_c();
   SmartPtr<const Vector> y_d = ip_data_->curr()->y_d();
   SmartPtr<const Vector> z_L = ip_data_->curr()->z_L();
   SmartPtr<const Vector> z_U = ip_data_->curr()->z_U();

   std::vector<const TaggedObject*> deps(5);
   deps[0] = GetRawPtr(x);
   deps[1] = GetRawPtr(y_c);
   deps[2] = GetRawPtr(y_d);
   deps[3] = GetRawPtr(z_L);
   deps[4] = GetRawPtr(z_U);

   if( !curr_grad_lag_x_cache_.GetCachedResult(result, deps) )
   {
      if( !trial_grad_lag_x_cache_.GetCachedResult(result, deps) )
      {
         SmartPtr<Vector> tmp = x->MakeNew();
         DBG_PRINT_VECTOR(2, "curr_grad_f", *curr_grad_f());
         tmp->Copy(*curr_grad_f());
         tmp->AddTwoVectors(1., *curr_jac_cT_times_curr_y_c(), 1., *curr_jac_dT_times_curr_y_d(), 1.);
         DBG_PRINT_VECTOR(2, "jac_cT*y_c", *curr_jac_cT_times_curr_y_c());
         DBG_PRINT_VECTOR(2, "jac_dT*y_d", *curr_jac_dT_times_curr_y_d());
         ip_nlp_->Px_L()->MultVector(-1., *z_L, 1., *tmp);
         ip_nlp_->Px_U()->MultVector(1., *z_U, 1., *tmp);
         result = ConstPtr(tmp);
      }
      curr_grad_lag_x_cache_.AddCachedResult(result, deps);
   }

   return result;
}

SmartPtr<const Vector> IpoptCalculatedQuantities::trial_grad_lag_x()
{
   DBG_START_METH("IpoptCalculatedQuantities::trial_grad_lag_x()",
                  dbg_verbosity);
   SmartPtr<const Vector> result;

   SmartPtr<const Vector> x = ip_data_->trial()->x();
   SmartPtr<const Vector> y_c = ip_data_->trial()->y_c();
   SmartPtr<const Vector> y_d = ip_data_->trial()->y_d();
   SmartPtr<const Vector> z_L = ip_data_->trial()->z_L();
   SmartPtr<const Vector> z_U = ip_data_->trial()->z_U();

   std::vector<const TaggedObject*> deps(5);
   deps[0] = GetRawPtr(x);
   deps[1] = GetRawPtr(y_c);
   deps[2] = GetRawPtr(y_d);
   deps[3] = GetRawPtr(z_L);
   deps[4] = GetRawPtr(z_U);

   if( !trial_grad_lag_x_cache_.GetCachedResult(result, deps) )
   {
      if( !curr_grad_lag_x_cache_.GetCachedResult(result, deps) )
      {
         SmartPtr<Vector> tmp = x->MakeNew();
         DBG_PRINT_VECTOR(2, "trial_grad_f", *trial_grad_f());
         tmp->Copy(*trial_grad_f());
         tmp->AddTwoVectors(1., *trial_jac_cT_times_trial_y_c(), 1., *trial_jac_dT_times_trial_y_d(), 1.);
         ip_nlp_->Px_L()->MultVector(-1., *z_L, 1., *tmp);
         ip_nlp_->Px_U()->MultVector(1., *z_U, 1., *tmp);
         result = ConstPtr(tmp);
      }
      trial_grad_lag_x_cache_.AddCachedResult(result, deps);
   }

   return result;
}

SmartPtr<const Vector> IpoptCalculatedQuantities::curr_grad_lag_s()
{
   DBG_START_METH("IpoptCalculatedQuantities::curr_grad_lag_s()",
                  dbg_verbosity);
   SmartPtr<const Vector> result;

   SmartPtr<const Vector> y_d = ip_data_->curr()->y_d();
   SmartPtr<const Vector> v_L = ip_data_->curr()->v_L();
   SmartPtr<const Vector> v_U = ip_data_->curr()->v_U();

   std::vector<const TaggedObject*> deps(3);
   deps[0] = GetRawPtr(y_d);
   deps[1] = GetRawPtr(v_L);
   deps[2] = GetRawPtr(v_U);

   if( !curr_grad_lag_s_cache_.GetCachedResult(result, deps) )
   {
      if( !trial_grad_lag_s_cache_.GetCachedResult(result, deps) )
      {
         SmartPtr<Vector> tmp = y_d->MakeNew();
         ip_nlp_->Pd_U()->MultVector(1., *v_U, 0., *tmp);
         ip_nlp_->Pd_L()->MultVector(-1., *v_L, 1., *tmp);
         tmp->Axpy(-1., *y_d);
         result = ConstPtr(tmp);
      }
      curr_grad_lag_s_cache_.AddCachedResult(result, deps);
   }

   return result;
}

SmartPtr<const Vector> IpoptCalculatedQuantities::trial_grad_lag_s()
{
   DBG_START_METH("IpoptCalculatedQuantities::trial_grad_lag_s()",
                  dbg_verbosity);
   SmartPtr<const Vector> result;

   SmartPtr<const Vector> y_d = ip_data_->trial()->y_d();
   SmartPtr<const Vector> v_L = ip_data_->trial()->v_L();
   SmartPtr<const Vector> v_U = ip_data_->trial()->v_U();

   std::vector<const TaggedObject*> deps(3);
   deps[0] = GetRawPtr(y_d);
   deps[1] = GetRawPtr(v_L);
   deps[2] = GetRawPtr(v_U);

   if( !trial_grad_lag_s_cache_.GetCachedResult(result, deps) )
   {
      if( !curr_grad_lag_s_cache_.GetCachedResult(result, deps) )
      {
         SmartPtr<Vector> tmp = y_d->MakeNew();
         ip_nlp_->Pd_U()->MultVector(1., *v_U, 0., *tmp);
         ip_nlp_->Pd_L()->MultVector(-1., *v_L, 1., *tmp);
         tmp->Axpy(-1., *y_d);
         result = ConstPtr(tmp);
      }
      trial_grad_lag_s_cache_.AddCachedResult(result, deps);
   }

   return result;
}

SmartPtr<const Vector> IpoptCalculatedQuantities::curr_grad_lag_with_damping_x()
{
   DBG_START_METH("IpoptCalculatedQuantities::curr_grad_lag_with_damping_x()",
                  dbg_verbosity);

   /* If no damping is used, just return the gradient of the regular
    Lagrangian function */
   if( kappa_d_ == 0. )
   {
      return curr_grad_lag_x();
   }

   SmartPtr<const Vector> result;

   SmartPtr<const Vector> x = ip_data_->curr()->x();
   SmartPtr<const Vector> y_c = ip_data_->curr()->y_c();
   SmartPtr<const Vector> y_d = ip_data_->curr()->y_d();
   SmartPtr<const Vector> z_L = ip_data_->curr()->z_L();
   SmartPtr<const Vector> z_U = ip_data_->curr()->z_U();
   Number mu = ip_data_->curr_mu();

   std::vector<const TaggedObject*> deps(5);
   deps[0] = GetRawPtr(x);
   deps[1] = GetRawPtr(y_c);
   deps[2] = GetRawPtr(y_d);
   deps[3] = GetRawPtr(z_L);
   deps[4] = GetRawPtr(z_U);
   std::vector<Number> sdeps(1);
   sdeps[0] = mu;

   if( !curr_grad_lag_with_damping_x_cache_.GetCachedResult(result, deps, sdeps) )
   {
      SmartPtr<Vector> tmp = x->MakeNew();
      tmp->Copy(*curr_grad_lag_x());

      SmartPtr<const Vector> dampind_x_L;
      SmartPtr<const Vector> dampind_x_U;
      SmartPtr<const Vector> dampind_s_L;
      SmartPtr<const Vector> dampind_s_U;
      ComputeDampingIndicators(dampind_x_L, dampind_x_U, dampind_s_L, dampind_s_U);

      ip_nlp_->Px_L()->MultVector(kappa_d_ * mu, *dampind_x_L, 1., *tmp);
      ip_nlp_->Px_U()->MultVector(-kappa_d_ * mu, *dampind_x_U, 1., *tmp);

      result = ConstPtr(tmp);
      curr_grad_lag_with_damping_x_cache_.AddCachedResult(result, deps, sdeps);
   }

   return result;
}

SmartPtr<const Vector> IpoptCalculatedQuantities::curr_grad_lag_with_damping_s()
{
   DBG_START_METH("IpoptCalculatedQuantities::curr_grad_lag_with_damping_s()",
                  dbg_verbosity);

   /* If no damping is used, just return the gradient of the regular
    Lagrangian function */
   if( kappa_d_ == 0. )
   {
      return curr_grad_lag_s();
   }

   SmartPtr<const Vector> result;

   SmartPtr<const Vector> y_d = ip_data_->curr()->y_d();
   SmartPtr<const Vector> v_L = ip_data_->curr()->v_L();
   SmartPtr<const Vector> v_U = ip_data_->curr()->v_U();
   Number mu = ip_data_->curr_mu();

   std::vector<const TaggedObject*> deps(3);
   deps[0] = GetRawPtr(y_d);
   deps[1] = GetRawPtr(v_L);
   deps[2] = GetRawPtr(v_U);
   std::vector<Number> sdeps(1);
   sdeps[0] = mu;

   if( !curr_grad_lag_with_damping_s_cache_.GetCachedResult(result, deps, sdeps) )
   {
      SmartPtr<Vector> tmp = y_d->MakeNew();
      tmp->Copy(*curr_grad_lag_s());

      SmartPtr<const Vector> dampind_x_L;
      SmartPtr<const Vector> dampind_x_U;
      SmartPtr<const Vector> dampind_s_L;
      SmartPtr<const Vector> dampind_s_U;
      ComputeDampingIndicators(dampind_x_L, dampind_x_U, dampind_s_L, dampind_s_U);

      ip_nlp_->Pd_L()->MultVector(kappa_d_ * mu, *dampind_s_L, 1., *tmp);
      ip_nlp_->Pd_U()->MultVector(-kappa_d_ * mu, *dampind_s_U, 1., *tmp);

      result = ConstPtr(tmp);
      curr_grad_lag_with_damping_s_cache_.AddCachedResult(result, deps, sdeps);
   }

   return result;
}

SmartPtr<const Vector> IpoptCalculatedQuantities::CalcCompl(
   const Vector& slack,
   const Vector& mult
)
{
   DBG_START_METH("IpoptCalculatedQuantities::CalcCompl()",
                  dbg_verbosity);
   SmartPtr<Vector> result = slack.MakeNew();
   result->Copy(slack);
   result->ElementWiseMultiply(mult);
   return ConstPtr(result);
}

SmartPtr<const Vector> IpoptCalculatedQuantities::curr_compl_x_L()
{
   DBG_START_METH("IpoptCalculatedQuantities::curr_compl_x_L()",
                  dbg_verbosity);
   SmartPtr<const Vector> result;

   SmartPtr<const Vector> slack = curr_slack_x_L();
   SmartPtr<const Vector> mult = ip_data_->curr()->z_L();
   DBG_PRINT_VECTOR(2, "slack_x_L", *slack);
   DBG_PRINT_VECTOR(2, "z_L", *mult);

   if( !curr_compl_x_L_cache_.GetCachedResult2Dep(result, *slack, *mult) )
   {
      if( !trial_compl_x_L_cache_.GetCachedResult2Dep(result, *slack, *mult) )
      {
         result = CalcCompl(*slack, *mult);
      }
      curr_compl_x_L_cache_.AddCachedResult2Dep(result, *slack, *mult);
   }
   return result;
}

SmartPtr<const Vector> IpoptCalculatedQuantities::trial_compl_x_L()
{
   DBG_START_METH("IpoptCalculatedQuantities::trial_compl_x_L()",
                  dbg_verbosity);
   SmartPtr<const Vector> result;

   SmartPtr<const Vector> slack = trial_slack_x_L();
   SmartPtr<const Vector> mult = ip_data_->trial()->z_L();
   DBG_PRINT_VECTOR(2, "slack_x_L", *slack);
   DBG_PRINT_VECTOR(2, "z_L", *mult);

   if( !trial_compl_x_L_cache_.GetCachedResult2Dep(result, *slack, *mult) )
   {
      if( !curr_compl_x_L_cache_.GetCachedResult2Dep(result, *slack, *mult) )
      {
         result = CalcCompl(*slack, *mult);
      }
      trial_compl_x_L_cache_.AddCachedResult2Dep(result, *slack, *mult);
   }
   return result;
}

SmartPtr<const Vector> IpoptCalculatedQuantities::curr_compl_x_U()
{
   DBG_START_METH("IpoptCalculatedQuantities::curr_compl_x_U()",
                  dbg_verbosity);
   SmartPtr<const Vector> result;

   SmartPtr<const Vector> slack = curr_slack_x_U();
   SmartPtr<const Vector> mult = ip_data_->curr()->z_U();

   if( !curr_compl_x_U_cache_.GetCachedResult2Dep(result, *slack, *mult) )
   {
      if( !trial_compl_x_U_cache_.GetCachedResult2Dep(result, *slack, *mult) )
      {
         result = CalcCompl(*slack, *mult);
      }
      curr_compl_x_U_cache_.AddCachedResult2Dep(result, *slack, *mult);
   }
   return result;
}

SmartPtr<const Vector> IpoptCalculatedQuantities::trial_compl_x_U()
{
   DBG_START_METH("IpoptCalculatedQuantities::trial_compl_x_U()",
                  dbg_verbosity);
   SmartPtr<const Vector> result;

   SmartPtr<const Vector> slack = trial_slack_x_U();
   SmartPtr<const Vector> mult = ip_data_->trial()->z_U();

   if( !trial_compl_x_U_cache_.GetCachedResult2Dep(result, *slack, *mult) )
   {
      if( !curr_compl_x_U_cache_.GetCachedResult2Dep(result, *slack, *mult) )
      {
         result = CalcCompl(*slack, *mult);
      }
      trial_compl_x_U_cache_.AddCachedResult2Dep(result, *slack, *mult);
   }
   return result;
}

SmartPtr<const Vector> IpoptCalculatedQuantities::curr_compl_s_L()
{
   DBG_START_METH("IpoptCalculatedQuantities::curr_compl_s_L()",
                  dbg_verbosity);
   SmartPtr<const Vector> result;

   SmartPtr<const Vector> slack = curr_slack_s_L();
   SmartPtr<const Vector> mult = ip_data_->curr()->v_L();

   if( !curr_compl_s_L_cache_.GetCachedResult2Dep(result, *slack, *mult) )
   {
      if( !trial_compl_s_L_cache_.GetCachedResult2Dep(result, *slack, *mult) )
      {
         result = CalcCompl(*slack, *mult);
      }
      curr_compl_s_L_cache_.GetCachedResult2Dep(result, *slack, *mult);
   }
   return result;
}

SmartPtr<const Vector> IpoptCalculatedQuantities::trial_compl_s_L()
{
   DBG_START_METH("IpoptCalculatedQuantities::trial_compl_s_L()",
                  dbg_verbosity);
   SmartPtr<const Vector> result;

   SmartPtr<const Vector> slack = trial_slack_s_L();
   SmartPtr<const Vector> mult = ip_data_->trial()->v_L();

   if( !trial_compl_s_L_cache_.GetCachedResult2Dep(result, *slack, *mult) )
   {
      if( !curr_compl_s_L_cache_.GetCachedResult2Dep(result, *slack, *mult) )
      {
         result = CalcCompl(*slack, *mult);
      }
      trial_compl_s_L_cache_.GetCachedResult2Dep(result, *slack, *mult);
   }
   return result;
}

SmartPtr<const Vector> IpoptCalculatedQuantities::curr_compl_s_U()
{
   DBG_START_METH("IpoptCalculatedQuantities::curr_compl_s_U()",
                  dbg_verbosity);
   SmartPtr<const Vector> result;

   SmartPtr<const Vector> slack = curr_slack_s_U();
   SmartPtr<const Vector> mult = ip_data_->curr()->v_U();

   if( !curr_compl_s_U_cache_.GetCachedResult2Dep(result, *slack, *mult) )
   {
      if( !trial_compl_s_U_cache_.GetCachedResult2Dep(result, *slack, *mult) )
      {
         result = CalcCompl(*slack, *mult);
      }
      curr_compl_s_U_cache_.GetCachedResult2Dep(result, *slack, *mult);
   }
   return result;
}

SmartPtr<const Vector> IpoptCalculatedQuantities::trial_compl_s_U()
{
   DBG_START_METH("IpoptCalculatedQuantities::trial_compl_s_U()",
                  dbg_verbosity);
   SmartPtr<const Vector> result;

   SmartPtr<const Vector> slack = trial_slack_s_U();
   SmartPtr<const Vector> mult = ip_data_->trial()->v_U();

   if( !trial_compl_s_U_cache_.GetCachedResult2Dep(result, *slack, *mult) )
   {
      if( !curr_compl_s_U_cache_.GetCachedResult2Dep(result, *slack, *mult) )
      {
         result = CalcCompl(*slack, *mult);
      }
      trial_compl_s_U_cache_.GetCachedResult2Dep(result, *slack, *mult);
   }
   return result;
}

SmartPtr<const Vector> IpoptCalculatedQuantities::curr_relaxed_compl_x_L()
{
   DBG_START_METH("IpoptCalculatedQuantities::curr_relaxed_compl_x_L()",
                  dbg_verbosity);
   SmartPtr<const Vector> result;

   SmartPtr<const Vector> slack = curr_slack_x_L();
   SmartPtr<const Vector> mult = ip_data_->curr()->z_L();
   std::vector<const TaggedObject*> tdeps(2);
   tdeps[0] = GetRawPtr(slack);
   tdeps[1] = GetRawPtr(mult);

   Number mu = ip_data_->curr_mu();
   std::vector<Number> sdeps(1);
   sdeps[0] = mu;

   if( !curr_relaxed_compl_x_L_cache_.GetCachedResult(result, tdeps, sdeps) )
   {
      SmartPtr<Vector> tmp = slack->MakeNew();
      tmp->Copy(*curr_compl_x_L());
      tmp->AddScalar(-mu);
      result = ConstPtr(tmp);
      curr_relaxed_compl_x_L_cache_.AddCachedResult(result, tdeps, sdeps);
   }
   return result;
}

SmartPtr<const Vector> IpoptCalculatedQuantities::curr_relaxed_compl_x_U()
{
   DBG_START_METH("IpoptCalculatedQuantities::curr_relaxed_compl_x_U()",
                  dbg_verbosity);
   SmartPtr<const Vector> result;

   SmartPtr<const Vector> slack = curr_slack_x_U();
   SmartPtr<const Vector> mult = ip_data_->curr()->z_U();
   std::vector<const TaggedObject*> tdeps(2);
   tdeps[0] = GetRawPtr(slack);
   tdeps[1] = GetRawPtr(mult);

   Number mu = ip_data_->curr_mu();
   std::vector<Number> sdeps(1);
   sdeps[0] = mu;

   if( !curr_relaxed_compl_x_U_cache_.GetCachedResult(result, tdeps, sdeps) )
   {
      SmartPtr<Vector> tmp = slack->MakeNew();
      tmp->Copy(*curr_compl_x_U());
      tmp->AddScalar(-mu);
      result = ConstPtr(tmp);
      curr_relaxed_compl_x_U_cache_.AddCachedResult(result, tdeps, sdeps);
   }
   return result;
}

SmartPtr<const Vector> IpoptCalculatedQuantities::curr_relaxed_compl_s_L()
{
   DBG_START_METH("IpoptCalculatedQuantities::curr_relaxed_compl_s_L()",
                  dbg_verbosity);
   SmartPtr<const Vector> result;

   SmartPtr<const Vector> slack = curr_slack_s_L();
   SmartPtr<const Vector> mult = ip_data_->curr()->v_L();
   std::vector<const TaggedObject*> tdeps(2);
   tdeps[0] = GetRawPtr(slack);
   tdeps[1] = GetRawPtr(mult);

   Number mu = ip_data_->curr_mu();
   std::vector<Number> sdeps(1);
   sdeps[0] = mu;

   if( !curr_relaxed_compl_s_L_cache_.GetCachedResult(result, tdeps, sdeps) )
   {
      SmartPtr<Vector> tmp = slack->MakeNew();
      tmp->Copy(*curr_compl_s_L());
      tmp->AddScalar(-mu);
      result = ConstPtr(tmp);
      curr_relaxed_compl_s_L_cache_.AddCachedResult(result, tdeps, sdeps);
   }
   return result;
}

SmartPtr<const Vector> IpoptCalculatedQuantities::curr_relaxed_compl_s_U()
{
   DBG_START_METH("IpoptCalculatedQuantities::curr_relaxed_compl_s_U()",
                  dbg_verbosity);
   SmartPtr<const Vector> result;

   SmartPtr<const Vector> slack = curr_slack_s_U();
   SmartPtr<const Vector> mult = ip_data_->curr()->v_U();
   std::vector<const TaggedObject*> tdeps(2);
   tdeps[0] = GetRawPtr(slack);
   tdeps[1] = GetRawPtr(mult);

   Number mu = ip_data_->curr_mu();
   std::vector<Number> sdeps(1);
   sdeps[0] = mu;

   if( !curr_relaxed_compl_s_U_cache_.GetCachedResult(result, tdeps, sdeps) )
   {
      SmartPtr<Vector> tmp = slack->MakeNew();
      tmp->Copy(*curr_compl_s_U());
      tmp->AddScalar(-mu);
      result = ConstPtr(tmp);
      curr_relaxed_compl_s_U_cache_.AddCachedResult(result, tdeps, sdeps);
   }
   return result;
}

Number IpoptCalculatedQuantities::CalcNormOfType(
   ENormType     NormType,
   const Vector& vec1,
   const Vector& vec2
)
{
   switch( NormType )
   {
      case NORM_1:
         return vec1.Asum() + vec2.Asum();
      case NORM_2:
         return sqrt(pow(vec1.Nrm2(), 2) + pow(vec2.Nrm2(), 2));
      case NORM_MAX:
         return Max(vec1.Amax(), vec2.Amax());
      default:
         DBG_ASSERT(false && "Unknown NormType.");
         return 0.0;
   }
}

Number IpoptCalculatedQuantities::CalcNormOfType(
   ENormType                            NormType,
   std::vector<SmartPtr<const Vector> > vecs
)
{
   Number result = 0.;

   switch( NormType )
   {
      case NORM_1:
         for( Index i = 0; i < (Index) vecs.size(); i++ )
         {
            result += vecs[i]->Asum();
         }
         break;
      case NORM_2:
         for( Index i = 0; i < (Index) vecs.size(); i++ )
         {
            Number nrm = vecs[i]->Nrm2();
            result += nrm * nrm;
         }
         result = sqrt(result);
         break;
      case NORM_MAX:
         for( Index i = 0; i < (Index) vecs.size(); i++ )
         {
            result = Max(result, vecs[i]->Amax());
         }
         break;
      default:
         DBG_ASSERT(false && "Unknown NormType.");
   }

   return result;
}

Number IpoptCalculatedQuantities::curr_primal_infeasibility(
   ENormType NormType
)
{
   DBG_START_METH("IpoptCalculatedQuantities::curr_primal_infeasibility()",
                  dbg_verbosity);
   Number result;

   SmartPtr<const Vector> x = ip_data_->curr()->x();
   SmartPtr<const Vector> s = ip_data_->curr()->s();

   DBG_PRINT_VECTOR(2, "x to eval", *x);
   DBG_PRINT_VECTOR(2, "s to eval", *s);
   DBG_PRINT((1, "NormType = %d\n", NormType))

   std::vector<const TaggedObject*> deps(2);
   deps[0] = GetRawPtr(x);
   deps[1] = GetRawPtr(s);
   std::vector<Number> sdeps(1);
   sdeps[0] = (Number) NormType;

   if( !curr_primal_infeasibility_cache_.GetCachedResult(result, deps, sdeps) )
   {
      if( !trial_primal_infeasibility_cache_.GetCachedResult(result, deps, sdeps) )
      {
         DBG_PRINT((1, "Recomputing recomputing infeasibility.\n"));
         SmartPtr<const Vector> c = curr_c();
         SmartPtr<const Vector> d_minus_s = curr_d_minus_s();

         DBG_PRINT_VECTOR(2, "c", *c);
         DBG_PRINT_VECTOR(2, "d_minus_s", *d_minus_s);

         result = CalcNormOfType(NormType, *c, *d_minus_s);

      }
      curr_primal_infeasibility_cache_.AddCachedResult(result, deps, sdeps);
   }

   DBG_PRINT((1, "result = %e\n", result));
   return result;
}

Number IpoptCalculatedQuantities::trial_primal_infeasibility(
   ENormType NormType
)
{
   DBG_START_METH("IpoptCalculatedQuantities::trial_primal_infeasibility()",
                  dbg_verbosity);
   Number result;

   SmartPtr<const Vector> x = ip_data_->trial()->x();
   SmartPtr<const Vector> s = ip_data_->trial()->s();

   DBG_PRINT_VECTOR(2, "x to eval", *x);
   DBG_PRINT_VECTOR(2, "s to eval", *s);
   DBG_PRINT((1, "NormType = %d\n", NormType))

   std::vector<const TaggedObject*> deps(2);
   deps[0] = GetRawPtr(x);
   deps[1] = GetRawPtr(s);
   std::vector<Number> sdeps(1);
   sdeps[0] = (Number) NormType;

   if( !trial_primal_infeasibility_cache_.GetCachedResult(result, deps, sdeps) )
   {
      if( !curr_primal_infeasibility_cache_.GetCachedResult(result, deps, sdeps) )
      {
         DBG_PRINT((1, "Recomputing recomputing infeasibility.\n"));
         SmartPtr<const Vector> c = trial_c();
         SmartPtr<const Vector> d_minus_s = trial_d_minus_s();

         DBG_PRINT_VECTOR(2, "c", *c);
         DBG_PRINT_VECTOR(2, "d_minus_s", *d_minus_s);

         result = CalcNormOfType(NormType, *c, *d_minus_s);
      }
      trial_primal_infeasibility_cache_.AddCachedResult(result, deps, sdeps);
   }

   DBG_PRINT((1, "result = %e\n", result));
   return result;
}

Number IpoptCalculatedQuantities::curr_dual_infeasibility(
   ENormType NormType
)
{
   DBG_START_METH("IpoptCalculatedQuantities::curr_dual_infeasibility()",
                  dbg_verbosity);
   Number result;

   SmartPtr<const Vector> x = ip_data_->curr()->x();
   SmartPtr<const Vector> s = ip_data_->curr()->s();
   SmartPtr<const Vector> y_c = ip_data_->curr()->y_c();
   SmartPtr<const Vector> y_d = ip_data_->curr()->y_d();
   SmartPtr<const Vector> z_L = ip_data_->curr()->z_L();
   SmartPtr<const Vector> z_U = ip_data_->curr()->z_U();
   SmartPtr<const Vector> v_L = ip_data_->curr()->v_L();
   SmartPtr<const Vector> v_U = ip_data_->curr()->v_U();

   std::vector<const TaggedObject*> deps(8);
   deps[0] = GetRawPtr(x);
   deps[1] = GetRawPtr(s);
   deps[2] = GetRawPtr(y_c);
   deps[3] = GetRawPtr(y_d);
   deps[4] = GetRawPtr(z_L);
   deps[5] = GetRawPtr(z_U);
   deps[6] = GetRawPtr(v_L);
   deps[7] = GetRawPtr(v_U);
   std::vector<Number> sdeps(1);
   sdeps[0] = (Number) NormType;

   if( !curr_dual_infeasibility_cache_.GetCachedResult(result, deps, sdeps) )
   {
      if( !trial_dual_infeasibility_cache_.GetCachedResult(result, deps, sdeps) )
      {
         SmartPtr<const Vector> grad_lag_x = curr_grad_lag_x();
         SmartPtr<const Vector> grad_lag_s = curr_grad_lag_s();
         DBG_PRINT_VECTOR(2, "grad_lag_x", *grad_lag_x);
         DBG_PRINT_VECTOR(2, "grad_lag_s", *grad_lag_s);

         result = CalcNormOfType(NormType, *grad_lag_x, *grad_lag_s);
      }
      curr_dual_infeasibility_cache_.AddCachedResult(result, deps, sdeps);
   }

   return result;
}

Number IpoptCalculatedQuantities::trial_dual_infeasibility(
   ENormType NormType
)
{
   DBG_START_METH("IpoptCalculatedQuantities::trial_dual_infeasibility()",
                  dbg_verbosity);
   Number result;

   SmartPtr<const Vector> x = ip_data_->trial()->x();
   SmartPtr<const Vector> s = ip_data_->trial()->s();
   SmartPtr<const Vector> y_c = ip_data_->trial()->y_c();
   SmartPtr<const Vector> y_d = ip_data_->trial()->y_d();
   SmartPtr<const Vector> z_L = ip_data_->trial()->z_L();
   SmartPtr<const Vector> z_U = ip_data_->trial()->z_U();
   SmartPtr<const Vector> v_L = ip_data_->trial()->v_L();
   SmartPtr<const Vector> v_U = ip_data_->trial()->v_U();

   std::vector<const TaggedObject*> deps(8);
   deps[0] = GetRawPtr(x);
   deps[1] = GetRawPtr(s);
   deps[2] = GetRawPtr(y_c);
   deps[3] = GetRawPtr(y_d);
   deps[4] = GetRawPtr(z_L);
   deps[5] = GetRawPtr(z_U);
   deps[6] = GetRawPtr(v_L);
   deps[7] = GetRawPtr(v_U);
   std::vector<Number> sdeps(1);
   sdeps[0] = (Number) NormType;

   if( !trial_dual_infeasibility_cache_.GetCachedResult(result, deps, sdeps) )
   {
      if( !curr_dual_infeasibility_cache_.GetCachedResult(result, deps, sdeps) )
      {
         SmartPtr<const Vector> grad_lag_x = trial_grad_lag_x();
         SmartPtr<const Vector> grad_lag_s = trial_grad_lag_s();
         DBG_PRINT_VECTOR(2, "grad_lag_x", *grad_lag_x);
         DBG_PRINT_VECTOR(2, "grad_lag_s", *grad_lag_s);

         result = CalcNormOfType(NormType, *grad_lag_x, *grad_lag_s);

      }
      trial_dual_infeasibility_cache_.AddCachedResult(result, deps, sdeps);
   }

   return result;
}

Number IpoptCalculatedQuantities::unscaled_curr_dual_infeasibility(
   ENormType NormType
)
{
   DBG_START_METH("IpoptCalculatedQuantities::unscaled_curr_dual_infeasibility()",
                  dbg_verbosity);
   Number result;

   SmartPtr<const Vector> x = ip_data_->curr()->x();
   SmartPtr<const Vector> s = ip_data_->curr()->s();
   SmartPtr<const Vector> y_c = ip_data_->curr()->y_c();
   SmartPtr<const Vector> y_d = ip_data_->curr()->y_d();
   SmartPtr<const Vector> z_L = ip_data_->curr()->z_L();
   SmartPtr<const Vector> z_U = ip_data_->curr()->z_U();
   SmartPtr<const Vector> v_L = ip_data_->curr()->v_L();
   SmartPtr<const Vector> v_U = ip_data_->curr()->v_U();

   std::vector<const TaggedObject*> deps(8);
   deps[0] = GetRawPtr(x);
   deps[1] = GetRawPtr(s);
   deps[2] = GetRawPtr(y_c);
   deps[3] = GetRawPtr(y_d);
   deps[4] = GetRawPtr(z_L);
   deps[5] = GetRawPtr(z_U);
   deps[6] = GetRawPtr(v_L);
   deps[7] = GetRawPtr(v_U);
   std::vector<Number> sdeps(1);
   sdeps[0] = (Number) NormType;

   if( !unscaled_curr_dual_infeasibility_cache_.GetCachedResult(result, deps, sdeps) )
   {
      SmartPtr<const Vector> grad_lag_x = ip_nlp_->NLP_scaling()->unapply_grad_obj_scaling(curr_grad_lag_x());

      Number obj_unscal = ip_nlp_->NLP_scaling()->unapply_obj_scaling(1.);
      SmartPtr<const Vector> grad_lag_s;
      if( obj_unscal != 1. )
      {
         SmartPtr<Vector> tmp = ip_nlp_->NLP_scaling()->apply_vector_scaling_d_NonConst(ConstPtr(curr_grad_lag_s()));
         tmp->Scal(obj_unscal);
         grad_lag_s = ConstPtr(tmp);
      }
      else
      {
         grad_lag_s = ip_nlp_->NLP_scaling()->apply_vector_scaling_d(curr_grad_lag_s());
      }

      result = CalcNormOfType(NormType, *grad_lag_x, *grad_lag_s);
      unscaled_curr_dual_infeasibility_cache_.AddCachedResult(result, deps, sdeps);
   }

   return result;
}

Number IpoptCalculatedQuantities::curr_complementarity(
   Number    mu,
   ENormType NormType
)
{
   DBG_START_METH("IpoptCalculatedQuantities::curr_complementarity()",
                  dbg_verbosity);
   Number result;

   SmartPtr<const Vector> x = ip_data_->curr()->x();
   SmartPtr<const Vector> s = ip_data_->curr()->s();
   SmartPtr<const Vector> z_L = ip_data_->curr()->z_L();
   SmartPtr<const Vector> z_U = ip_data_->curr()->z_U();
   SmartPtr<const Vector> v_L = ip_data_->curr()->v_L();
   SmartPtr<const Vector> v_U = ip_data_->curr()->v_U();

   std::vector<const TaggedObject*> deps(6);
   deps[0] = GetRawPtr(x);
   deps[1] = GetRawPtr(s);
   deps[2] = GetRawPtr(z_L);
   deps[3] = GetRawPtr(z_U);
   deps[4] = GetRawPtr(v_L);
   deps[5] = GetRawPtr(v_U);
   std::vector<Number> sdeps(2);
   sdeps[0] = (Number) NormType;
   sdeps[1] = mu;

   if( !curr_complementarity_cache_.GetCachedResult(result, deps, sdeps) )
   {
      if( !trial_complementarity_cache_.GetCachedResult(result, deps, sdeps) )
      {

         std::vector<SmartPtr<const Vector> > vecs(4);
         SmartPtr<const Vector> compl_x_L = curr_compl_x_L();
         SmartPtr<const Vector> compl_x_U = curr_compl_x_U();
         SmartPtr<const Vector> compl_s_L = curr_compl_s_L();
         SmartPtr<const Vector> compl_s_U = curr_compl_s_U();

         if( mu == .0 )
         {
            vecs[0] = GetRawPtr(compl_x_L);
            vecs[1] = GetRawPtr(compl_x_U);
            vecs[2] = GetRawPtr(compl_s_L);
            vecs[3] = GetRawPtr(compl_s_U);
         }
         else
         {
            SmartPtr<Vector> tmp = compl_x_L->MakeNew();
            tmp->Copy(*compl_x_L);
            tmp->AddScalar(-mu);
            vecs[0] = GetRawPtr(tmp);
            tmp = compl_x_U->MakeNew();
            tmp->Copy(*compl_x_U);
            tmp->AddScalar(-mu);
            vecs[1] = GetRawPtr(tmp);
            tmp = compl_s_L->MakeNew();
            tmp->Copy(*compl_s_L);
            tmp->AddScalar(-mu);
            vecs[2] = GetRawPtr(tmp);
            tmp = compl_s_U->MakeNew();
            tmp->Copy(*compl_s_U);
            tmp->AddScalar(-mu);
            vecs[3] = GetRawPtr(tmp);
         }

         result = CalcNormOfType(NormType, vecs);
      }

      curr_complementarity_cache_.AddCachedResult(result, deps, sdeps);
   }

   return result;
}

Number IpoptCalculatedQuantities::trial_complementarity(
   Number    mu,
   ENormType NormType
)
{
   DBG_START_METH("IpoptCalculatedQuantities::trial_complementarity()",
                  dbg_verbosity);
   Number result;

   SmartPtr<const Vector> x = ip_data_->trial()->x();
   SmartPtr<const Vector> s = ip_data_->trial()->s();
   SmartPtr<const Vector> z_L = ip_data_->trial()->z_L();
   SmartPtr<const Vector> z_U = ip_data_->trial()->z_U();
   SmartPtr<const Vector> v_L = ip_data_->trial()->v_L();
   SmartPtr<const Vector> v_U = ip_data_->trial()->v_U();

   std::vector<const TaggedObject*> deps(6);
   deps[0] = GetRawPtr(x);
   deps[1] = GetRawPtr(s);
   deps[2] = GetRawPtr(z_L);
   deps[3] = GetRawPtr(z_U);
   deps[4] = GetRawPtr(v_L);
   deps[5] = GetRawPtr(v_U);
   std::vector<Number> sdeps(2);
   sdeps[0] = (Number) NormType;
   sdeps[1] = mu;

   if( !trial_complementarity_cache_.GetCachedResult(result, deps, sdeps) )
   {
      if( !curr_complementarity_cache_.GetCachedResult(result, deps, sdeps) )
      {

         std::vector<SmartPtr<const Vector> > vecs(4);
         SmartPtr<const Vector> compl_x_L = trial_compl_x_L();
         SmartPtr<const Vector> compl_x_U = trial_compl_x_U();
         SmartPtr<const Vector> compl_s_L = trial_compl_s_L();
         SmartPtr<const Vector> compl_s_U = trial_compl_s_U();

         if( mu == .0 )
         {
            vecs[0] = GetRawPtr(compl_x_L);
            vecs[1] = GetRawPtr(compl_x_U);
            vecs[2] = GetRawPtr(compl_s_L);
            vecs[3] = GetRawPtr(compl_s_U);
         }
         else
         {
            SmartPtr<Vector> tmp = compl_x_L->MakeNew();
            tmp->Copy(*compl_x_L);
            tmp->AddScalar(-mu);
            vecs[0] = GetRawPtr(tmp);
            tmp = compl_x_U->MakeNew();
            tmp->Copy(*compl_x_U);
            tmp->AddScalar(-mu);
            vecs[1] = GetRawPtr(tmp);
            tmp = compl_s_L->MakeNew();
            tmp->Copy(*compl_s_L);
            tmp->AddScalar(-mu);
            vecs[2] = GetRawPtr(tmp);
            tmp = compl_s_U->MakeNew();
            tmp->Copy(*compl_s_U);
            tmp->AddScalar(-mu);
            vecs[3] = GetRawPtr(tmp);
         }

         result = CalcNormOfType(NormType, vecs);
      }

      trial_complementarity_cache_.AddCachedResult(result, deps, sdeps);
   }

   return result;
}

Number IpoptCalculatedQuantities::unscaled_curr_complementarity(
   Number    mu,
   ENormType NormType
)
{
   return ip_nlp_->NLP_scaling()->unapply_obj_scaling(curr_complementarity(mu, NormType));
}

Number IpoptCalculatedQuantities::CalcCentralityMeasure(
   const Vector& compl_x_L,
   const Vector& compl_x_U,
   const Vector& compl_s_L,
   const Vector& compl_s_U
)
{
   DBG_START_METH("IpoptCalculatedQuantities::CalcCentralityMeasure()",
                  dbg_verbosity);

   Number MinCompl = std::numeric_limits<Number>::max();
   bool have_bounds = false;

   Index n_compl_x_L = compl_x_L.Dim();
   Index n_compl_x_U = compl_x_U.Dim();
   Index n_compl_s_L = compl_s_L.Dim();
   Index n_compl_s_U = compl_s_U.Dim();

   // Compute the Minimum of all complementarities
   if( n_compl_x_L > 0 )
   {
      if( have_bounds )
      {
         MinCompl = Min(MinCompl, compl_x_L.Min());
      }
      else
      {
         MinCompl = compl_x_L.Min();
      }
      have_bounds = true;
   }
   if( n_compl_x_U > 0 )
   {
      if( have_bounds )
      {
         MinCompl = Min(MinCompl, compl_x_U.Min());
      }
      else
      {
         MinCompl = compl_x_U.Min();
      }
      have_bounds = true;
   }
   if( n_compl_s_L > 0 )
   {
      if( have_bounds )
      {
         MinCompl = Min(MinCompl, compl_s_L.Min());
      }
      else
      {
         MinCompl = compl_s_L.Min();
      }
      have_bounds = true;
   }
   if( n_compl_s_U > 0 )
   {
      if( have_bounds )
      {
         MinCompl = Min(MinCompl, compl_s_U.Min());
      }
      else
      {
         MinCompl = compl_s_U.Min();
      }
      have_bounds = true;
   }

   // If there are no bounds, just return 0.;
   if( !have_bounds )
   {
      return 0.;
   }

   DBG_PRINT_VECTOR(2, "compl_x_L", compl_x_L);
   DBG_PRINT_VECTOR(2, "compl_x_U", compl_x_U);
   DBG_PRINT_VECTOR(2, "compl_s_L", compl_s_L);
   DBG_PRINT_VECTOR(2, "compl_s_U", compl_s_U);

   DBG_ASSERT(MinCompl > 0. && "There is a zero complementarity entry");

   Number avrg_compl = (compl_x_L.Asum() + compl_x_U.Asum() + compl_s_L.Asum() + compl_s_U.Asum());
   DBG_PRINT((1, "sum_compl = %25.16e\n", avrg_compl));
   avrg_compl /= (n_compl_x_L + n_compl_x_U + n_compl_s_L + n_compl_s_U);
   DBG_PRINT((1, "avrg_compl = %25.16e\n", avrg_compl));
   DBG_PRINT((1, "MinCompl = %25.16e\n", MinCompl));

   Number xi = MinCompl / avrg_compl;
   // The folloking line added for the case that avrg_compl is
   // slighly smaller than MinCompl, due to numerical roundoff
   xi = Min(1., xi);

   return xi;
}

Number IpoptCalculatedQuantities::curr_centrality_measure()
{
   Number result;

   SmartPtr<const Vector> x = ip_data_->curr()->x();
   SmartPtr<const Vector> s = ip_data_->curr()->s();
   SmartPtr<const Vector> z_L = ip_data_->curr()->z_L();
   SmartPtr<const Vector> z_U = ip_data_->curr()->z_U();
   SmartPtr<const Vector> v_L = ip_data_->curr()->v_L();
   SmartPtr<const Vector> v_U = ip_data_->curr()->v_U();

   std::vector<const TaggedObject*> tdeps(6);
   tdeps[0] = GetRawPtr(x);
   tdeps[1] = GetRawPtr(s);
   tdeps[2] = GetRawPtr(z_L);
   tdeps[3] = GetRawPtr(z_U);
   tdeps[4] = GetRawPtr(v_L);
   tdeps[5] = GetRawPtr(v_U);

   if( !curr_centrality_measure_cache_.GetCachedResult(result, tdeps) )
   {
      SmartPtr<const Vector> compl_x_L = curr_compl_x_L();
      SmartPtr<const Vector> compl_x_U = curr_compl_x_U();
      SmartPtr<const Vector> compl_s_L = curr_compl_s_L();
      SmartPtr<const Vector> compl_s_U = curr_compl_s_U();

      result = CalcCentralityMeasure(*compl_x_L, *compl_x_U, *compl_s_L, *compl_s_U);

      curr_centrality_measure_cache_.AddCachedResult(result, tdeps);
   }
   return result;
}

Number IpoptCalculatedQuantities::curr_nlp_error()
{
   DBG_START_METH("IpoptCalculatedQuantities::curr_nlp_error()",
                  dbg_verbosity);
   DBG_ASSERT(initialize_called_);
   Number result;

   SmartPtr<const Vector> x = ip_data_->curr()->x();
   SmartPtr<const Vector> s = ip_data_->curr()->s();
   SmartPtr<const Vector> y_c = ip_data_->curr()->y_c();
   SmartPtr<const Vector> y_d = ip_data_->curr()->y_d();
   SmartPtr<const Vector> z_L = ip_data_->curr()->z_L();
   SmartPtr<const Vector> z_U = ip_data_->curr()->z_U();
   SmartPtr<const Vector> v_L = ip_data_->curr()->v_L();
   SmartPtr<const Vector> v_U = ip_data_->curr()->v_U();

   std::vector<const TaggedObject*> tdeps(8);
   tdeps[0] = GetRawPtr(x);
   tdeps[1] = GetRawPtr(s);
   tdeps[2] = GetRawPtr(y_c);
   tdeps[3] = GetRawPtr(y_d);
   tdeps[4] = GetRawPtr(z_L);
   tdeps[5] = GetRawPtr(z_U);
   tdeps[6] = GetRawPtr(v_L);
   tdeps[7] = GetRawPtr(v_U);

   if( !curr_nlp_error_cache_.GetCachedResult(result, tdeps) )
   {
      if( ip_data_->curr()->x()->Dim() == ip_data_->curr()->y_c()->Dim() )
      {
         // This is a square problem, we only need to consider the
         // infeasibility
         result = curr_nlp_constraint_violation(NORM_MAX);
      }
      else
      {
         Number s_d = 0;
         Number s_c = 0;
         ComputeOptimalityErrorScaling(*ip_data_->curr()->y_c(), *ip_data_->curr()->y_d(), *ip_data_->curr()->z_L(),
                                       *ip_data_->curr()->z_U(), *ip_data_->curr()->v_L(), *ip_data_->curr()->v_U(), s_max_, s_d, s_c);
         DBG_PRINT((1, "s_d = %lf, s_c = %lf\n", s_d, s_c));

         // Dual infeasibility
         DBG_PRINT((1, "curr_dual_infeasibility(NORM_MAX) = %8.2e\n",
                    curr_dual_infeasibility(NORM_MAX)));
         result = curr_dual_infeasibility(NORM_MAX) / s_d;
         /*
          // Primal infeasibility
          DBG_PRINT((1, "curr_primal_infeasibility(NORM_MAX) = %8.2e\n",
          curr_primal_infeasibility(NORM_MAX)));
          result = Max(result, curr_primal_infeasibility(NORM_MAX));
          */
         result = Max(result, curr_nlp_constraint_violation(NORM_MAX));
         // Complementarity
         DBG_PRINT((1, "curr_complementarity(mu_target_, NORM_MAX) = %8.2e\n",
                    curr_complementarity(mu_target_, NORM_MAX)));
         result = Max(result, curr_complementarity(mu_target_, NORM_MAX) / s_c);
      }

      curr_nlp_error_cache_.AddCachedResult(result, tdeps);
   }

   return result;
}

Number IpoptCalculatedQuantities::unscaled_curr_nlp_error()
{
   DBG_START_METH("IpoptCalculatedQuantities::unscaled_curr_nlp_error()",
                  dbg_verbosity);
   DBG_ASSERT(initialize_called_);
   Number result;

   SmartPtr<const Vector> x = ip_data_->curr()->x();
   SmartPtr<const Vector> s = ip_data_->curr()->s();
   SmartPtr<const Vector> y_c = ip_data_->curr()->y_c();
   SmartPtr<const Vector> y_d = ip_data_->curr()->y_d();
   SmartPtr<const Vector> z_L = ip_data_->curr()->z_L();
   SmartPtr<const Vector> z_U = ip_data_->curr()->z_U();
   SmartPtr<const Vector> v_L = ip_data_->curr()->v_L();
   SmartPtr<const Vector> v_U = ip_data_->curr()->v_U();

   std::vector<const TaggedObject*> tdeps(8);
   tdeps[0] = GetRawPtr(x);
   tdeps[1] = GetRawPtr(s);
   tdeps[2] = GetRawPtr(y_c);
   tdeps[3] = GetRawPtr(y_d);
   tdeps[4] = GetRawPtr(z_L);
   tdeps[5] = GetRawPtr(z_U);
   tdeps[6] = GetRawPtr(v_L);
   tdeps[7] = GetRawPtr(v_U);

   if( !unscaled_curr_nlp_error_cache_.GetCachedResult(result, tdeps) )
   {

      // Dual infeasibility
      result = unscaled_curr_dual_infeasibility(NORM_MAX);
      // Constraint violation
      result = Max(result, unscaled_curr_nlp_constraint_violation(NORM_MAX));
      // Complementarity (ToDo use unscaled?)
      DBG_PRINT((1, "curr_complementarity(mu_target_, NORM_MAX) = %8.2e\n",
                 curr_complementarity(mu_target_, NORM_MAX)));
      result = Max(result, unscaled_curr_complementarity(mu_target_, NORM_MAX));

      unscaled_curr_nlp_error_cache_.AddCachedResult(result, tdeps);
   }

   return result;
}

Number IpoptCalculatedQuantities::curr_barrier_error()
{
   DBG_START_METH("IpoptCalculatedQuantities::curr_barrier_error()",
                  dbg_verbosity);
   DBG_ASSERT(initialize_called_);
   Number result;

   SmartPtr<const Vector> x = ip_data_->curr()->x();
   SmartPtr<const Vector> s = ip_data_->curr()->s();
   SmartPtr<const Vector> y_c = ip_data_->curr()->y_c();
   SmartPtr<const Vector> y_d = ip_data_->curr()->y_d();
   SmartPtr<const Vector> z_L = ip_data_->curr()->z_L();
   SmartPtr<const Vector> z_U = ip_data_->curr()->z_U();
   SmartPtr<const Vector> v_L = ip_data_->curr()->v_L();
   SmartPtr<const Vector> v_U = ip_data_->curr()->v_U();
   Number mu = ip_data_->curr_mu();

   std::vector<const TaggedObject*> tdeps(8);
   tdeps[0] = GetRawPtr(x);
   tdeps[1] = GetRawPtr(s);
   tdeps[2] = GetRawPtr(y_c);
   tdeps[3] = GetRawPtr(y_d);
   tdeps[4] = GetRawPtr(z_L);
   tdeps[5] = GetRawPtr(z_U);
   tdeps[6] = GetRawPtr(v_L);
   tdeps[7] = GetRawPtr(v_U);
   std::vector<Number> sdeps(1);
   sdeps[0] = mu;

   if( !curr_barrier_error_cache_.GetCachedResult(result, tdeps, sdeps) )
   {
      Number s_d = 0;
      Number s_c = 0;
      ComputeOptimalityErrorScaling(*ip_data_->curr()->y_c(), *ip_data_->curr()->y_d(), *ip_data_->curr()->z_L(),
                                    *ip_data_->curr()->z_U(), *ip_data_->curr()->v_L(), *ip_data_->curr()->v_U(), s_max_, s_d, s_c);
      DBG_PRINT((1, "s_d = %lf, s_c = %lf\n", s_d, s_c));

      // Primal infeasibility
      result = curr_dual_infeasibility(NORM_MAX) / s_d;
      // Dual infeasibility
      result = Max(result, curr_primal_infeasibility(NORM_MAX));
      // Complementarity
      result = Max(result, curr_complementarity(mu, NORM_MAX) / s_c);

      curr_barrier_error_cache_.AddCachedResult(result, tdeps);
   }

   return result;
}

Number IpoptCalculatedQuantities::curr_primal_dual_system_error(
   Number mu
)
{
   DBG_START_METH("IpoptCalculatedQuantities::curr_primal_dual_system_error()",
                  dbg_verbosity);
   DBG_ASSERT(initialize_called_);
   Number result;

   SmartPtr<const Vector> x = ip_data_->curr()->x();
   SmartPtr<const Vector> s = ip_data_->curr()->s();
   SmartPtr<const Vector> y_c = ip_data_->curr()->y_c();
   SmartPtr<const Vector> y_d = ip_data_->curr()->y_d();
   SmartPtr<const Vector> z_L = ip_data_->curr()->z_L();
   SmartPtr<const Vector> z_U = ip_data_->curr()->z_U();
   SmartPtr<const Vector> v_L = ip_data_->curr()->v_L();
   SmartPtr<const Vector> v_U = ip_data_->curr()->v_U();

   std::vector<const TaggedObject*> tdeps(8);
   tdeps[0] = GetRawPtr(x);
   tdeps[1] = GetRawPtr(s);
   tdeps[2] = GetRawPtr(y_c);
   tdeps[3] = GetRawPtr(y_d);
   tdeps[4] = GetRawPtr(z_L);
   tdeps[5] = GetRawPtr(z_U);
   tdeps[6] = GetRawPtr(v_L);
   tdeps[7] = GetRawPtr(v_U);
   std::vector<Number> sdeps(1);
   sdeps[0] = mu;

   if( !curr_primal_dual_system_error_cache_.GetCachedResult(result, tdeps, sdeps) )
   {
      if( !trial_primal_dual_system_error_cache_.GetCachedResult(result, tdeps, sdeps) )
      {
         // For now we use the 1 norm, and scale each component by the number of entries...
         Index n_dual = x->Dim() + s->Dim();
         Number dual_inf = curr_dual_infeasibility(NORM_1) / ((Number) n_dual);

         Index n_primal = y_c->Dim() + y_d->Dim();
         Number primal_inf = 0.;
         if( n_primal > 0 )
         {
            primal_inf = curr_primal_infeasibility(NORM_1) / ((Number) n_primal);
         }

         Index n_cmpl = z_L->Dim() + z_U->Dim() + v_L->Dim() + v_U->Dim();
         Number cmpl = 0.;
         if( n_cmpl > 0 )
         {
            cmpl = curr_complementarity(mu, NORM_1) / ((Number) n_cmpl);
         }

         result = dual_inf + primal_inf + cmpl;
      }
      curr_primal_dual_system_error_cache_.AddCachedResult(result, tdeps, sdeps);
   }

   return result;
}

Number IpoptCalculatedQuantities::trial_primal_dual_system_error(
   Number mu
)
{
   DBG_START_METH("IpoptCalculatedQuantities::trial_primal_dual_system_error()",
                  dbg_verbosity);
   DBG_ASSERT(initialize_called_);
   Number result;

   SmartPtr<const Vector> x = ip_data_->trial()->x();
   SmartPtr<const Vector> s = ip_data_->trial()->s();
   SmartPtr<const Vector> y_c = ip_data_->trial()->y_c();
   SmartPtr<const Vector> y_d = ip_data_->trial()->y_d();
   SmartPtr<const Vector> z_L = ip_data_->trial()->z_L();
   SmartPtr<const Vector> z_U = ip_data_->trial()->z_U();
   SmartPtr<const Vector> v_L = ip_data_->trial()->v_L();
   SmartPtr<const Vector> v_U = ip_data_->trial()->v_U();

   std::vector<const TaggedObject*> tdeps(8);
   tdeps[0] = GetRawPtr(x);
   tdeps[1] = GetRawPtr(s);
   tdeps[2] = GetRawPtr(y_c);
   tdeps[3] = GetRawPtr(y_d);
   tdeps[4] = GetRawPtr(z_L);
   tdeps[5] = GetRawPtr(z_U);
   tdeps[6] = GetRawPtr(v_L);
   tdeps[7] = GetRawPtr(v_U);
   std::vector<Number> sdeps(1);
   sdeps[0] = mu;

   if( !trial_primal_dual_system_error_cache_.GetCachedResult(result, tdeps, sdeps) )
   {
      if( !curr_primal_dual_system_error_cache_.GetCachedResult(result, tdeps, sdeps) )
      {
         // For now we use the 1 norm, and scale each component by the number of entries...
         Index n_dual = x->Dim() + s->Dim();
         Number dual_inf = trial_dual_infeasibility(NORM_1) / ((Number) n_dual);

         Index n_primal = y_c->Dim() + y_d->Dim();
         Number primal_inf = 0.;
         if( n_primal > 0 )
         {
            primal_inf = trial_primal_infeasibility(NORM_1) / ((Number) n_primal);
         }

         Index n_cmpl = z_L->Dim() + z_U->Dim() + v_L->Dim() + v_U->Dim();
         Number cmpl = 0.;
         if( n_cmpl > 0 )
         {
            cmpl = trial_complementarity(mu, NORM_1) / ((Number) n_cmpl);
         }

         result = dual_inf + primal_inf + cmpl;
      }
      trial_primal_dual_system_error_cache_.AddCachedResult(result, tdeps, sdeps);
   }

   return result;
}

///////////////////////////////////////////////////////////////////////////
//                Fraction-to-the-boundary step sizes                    //
///////////////////////////////////////////////////////////////////////////

Number IpoptCalculatedQuantities::CalcFracToBound(
   const Vector& slack_L,
   Vector&       tmp_L,
   const Matrix& P_L,
   const Vector& slack_U,
   Vector&       tmp_U,
   const Matrix& P_U,
   const Vector& delta,
   Number        tau
)
{
   DBG_START_METH("IpoptCalculatedQuantities::CalcFracToBound",
                  dbg_verbosity);

   Number alpha_L = 1.0;
   Number alpha_U = 1.0;
   if( slack_L.Dim() > 0 )
   {
      P_L.TransMultVector(1.0, delta, 0.0, tmp_L);
      alpha_L = slack_L.FracToBound(tmp_L, tau);
   }

   if( slack_U.Dim() > 0 )
   {
      P_U.TransMultVector(-1.0, delta, 0.0, tmp_U);
      alpha_U = slack_U.FracToBound(tmp_U, tau);
   }

   DBG_PRINT((1, "alpha_L = %lf, alpha_U = %lf\n", alpha_L, alpha_U));
   DBG_ASSERT(alpha_L >= 0.0 && alpha_L <= 1.0
              && alpha_U >= 0.0 && alpha_U <= 1.0);

   return Min(alpha_L, alpha_U);
}

Number IpoptCalculatedQuantities::primal_frac_to_the_bound(
   Number        tau,
   const Vector& delta_x,
   const Vector& delta_s
)
{
   DBG_START_METH("IpoptCalculatedQuantities::primal_frac_to_the_bound",
                  dbg_verbosity);
   Number result;
   SmartPtr<const Vector> x = ip_data_->curr()->x();
   SmartPtr<const Vector> s = ip_data_->curr()->s();
   std::vector<const TaggedObject*> tdeps(4);
   tdeps[0] = GetRawPtr(x);
   tdeps[1] = GetRawPtr(s);
   tdeps[2] = &delta_x;
   tdeps[3] = &delta_s;

   std::vector<Number> sdeps(1);
   sdeps[0] = tau;

   if( !primal_frac_to_the_bound_cache_.GetCachedResult(result, tdeps, sdeps) )
   {
      result = Min(
                  CalcFracToBound(*curr_slack_x_L(), Tmp_x_L(), *ip_nlp_->Px_L(), *curr_slack_x_U(), Tmp_x_U(), *ip_nlp_->Px_U(),
                                  delta_x, tau),
                  CalcFracToBound(*curr_slack_s_L(), Tmp_s_L(), *ip_nlp_->Pd_L(), *curr_slack_s_U(), Tmp_s_U(), *ip_nlp_->Pd_U(),
                                  delta_s, tau));

      primal_frac_to_the_bound_cache_.AddCachedResult(result, tdeps, sdeps);
   }

   return result;
}

Number IpoptCalculatedQuantities::curr_primal_frac_to_the_bound(
   Number tau
)
{
   DBG_START_METH("IpoptCalculatedQuantities::curr_primal_frac_to_the_bound()",
                  dbg_verbosity);
   return primal_frac_to_the_bound(tau, *ip_data_->delta()->x(), *ip_data_->delta()->s());
}

Number IpoptCalculatedQuantities::uncached_dual_frac_to_the_bound(
   Number        tau,
   const Vector& delta_z_L,
   const Vector& delta_z_U,
   const Vector& delta_v_L,
   const Vector& delta_v_U
)
{
   DBG_START_METH("IpoptCalculatedQuantities::uncached_dual_frac_to_the_bound",
                  dbg_verbosity);
   Number result;

   result = ip_data_->curr()->z_L()->FracToBound(delta_z_L, tau);
   result = Min(result, ip_data_->curr()->z_U()->FracToBound(delta_z_U, tau));
   result = Min(result, ip_data_->curr()->v_L()->FracToBound(delta_v_L, tau));
   result = Min(result, ip_data_->curr()->v_U()->FracToBound(delta_v_U, tau));

   return result;
}

Number IpoptCalculatedQuantities::dual_frac_to_the_bound(
   Number        tau,
   const Vector& delta_z_L,
   const Vector& delta_z_U,
   const Vector& delta_v_L,
   const Vector& delta_v_U
)
{
   DBG_START_METH("IpoptCalculatedQuantities::dual_frac_to_the_bound",
                  dbg_verbosity);
   Number result;

   SmartPtr<const Vector> z_L = ip_data_->curr()->z_L();
   SmartPtr<const Vector> z_U = ip_data_->curr()->z_U();
   SmartPtr<const Vector> v_L = ip_data_->curr()->v_L();
   SmartPtr<const Vector> v_U = ip_data_->curr()->v_U();
   std::vector<const TaggedObject*> tdeps(8);
   tdeps[0] = GetRawPtr(z_L);
   tdeps[1] = GetRawPtr(z_U);
   tdeps[2] = GetRawPtr(v_L);
   tdeps[3] = GetRawPtr(v_U);
   tdeps[4] = &delta_z_L;
   tdeps[5] = &delta_z_U;
   tdeps[6] = &delta_v_L;
   tdeps[7] = &delta_v_U;

   std::vector<Number> sdeps(1);
   sdeps[0] = tau;

   if( !dual_frac_to_the_bound_cache_.GetCachedResult(result, tdeps, sdeps) )
   {
      result = z_L->FracToBound(delta_z_L, tau);
      result = Min(result, z_U->FracToBound(delta_z_U, tau));
      result = Min(result, v_L->FracToBound(delta_v_L, tau));
      result = Min(result, v_U->FracToBound(delta_v_U, tau));

      dual_frac_to_the_bound_cache_.AddCachedResult(result, tdeps, sdeps);
   }

   return result;
}

Number IpoptCalculatedQuantities::curr_dual_frac_to_the_bound(
   Number tau
)
{
   DBG_START_METH("IpoptCalculatedQuantities::curr_dual_frac_to_the_bound()",
                  dbg_verbosity);
   return dual_frac_to_the_bound(tau, *ip_data_->delta()->z_L(), *ip_data_->delta()->z_U(), *ip_data_->delta()->v_L(),
                                 *ip_data_->delta()->v_U());
}

Number IpoptCalculatedQuantities::uncached_slack_frac_to_the_bound(
   Number        tau,
   const Vector& delta_x_L,
   const Vector& delta_x_U,
   const Vector& delta_s_L,
   const Vector& delta_s_U
)
{
   DBG_START_METH("IpoptCalculatedQuantities::slack_frac_to_the_bound",
                  dbg_verbosity);
   Number result;

   SmartPtr<const Vector> x_L = curr_slack_x_L();
   SmartPtr<const Vector> x_U = curr_slack_x_U();
   SmartPtr<const Vector> s_L = curr_slack_s_L();
   SmartPtr<const Vector> s_U = curr_slack_s_U();

   result = x_L->FracToBound(delta_x_L, tau);
   result = Min(result, x_U->FracToBound(delta_x_U, tau));
   result = Min(result, s_L->FracToBound(delta_s_L, tau));
   result = Min(result, s_U->FracToBound(delta_s_U, tau));

   return result;
}

///////////////////////////////////////////////////////////////////////////
//                             Sigma Matrices                            //
///////////////////////////////////////////////////////////////////////////

SmartPtr<const Vector> IpoptCalculatedQuantities::curr_sigma_x()
{
   DBG_START_METH("IpoptCalculatedQuantities::curr_sigma_x()",
                  dbg_verbosity);
   SmartPtr<const Vector> result;
   SmartPtr<const Vector> x = ip_data_->curr()->x();
   SmartPtr<const Vector> z_L = ip_data_->curr()->z_L();
   SmartPtr<const Vector> z_U = ip_data_->curr()->z_U();

   if( !curr_sigma_x_cache_.GetCachedResult3Dep(result, *x, *z_L, *z_U) )
   {
      SmartPtr<Vector> sigma = x->MakeNew();

      sigma->Set(0.);
      ip_nlp_->Px_L()->AddMSinvZ(1., *curr_slack_x_L(), *z_L, *sigma);
      ip_nlp_->Px_U()->AddMSinvZ(1., *curr_slack_x_U(), *z_U, *sigma);

      DBG_PRINT_VECTOR(2, "sigma_x", *sigma);

      result = ConstPtr(sigma);
      curr_sigma_x_cache_.AddCachedResult3Dep(result, *x, *z_L, *z_U);
   }

   return result;
}

SmartPtr<const Vector> IpoptCalculatedQuantities::curr_sigma_s()
{
   DBG_START_METH("IpoptCalculatedQuantities::curr_sigma_s()",
                  dbg_verbosity);
   SmartPtr<const Vector> result;
   SmartPtr<const Vector> s = ip_data_->curr()->s();
   SmartPtr<const Vector> v_L = ip_data_->curr()->v_L();
   SmartPtr<const Vector> v_U = ip_data_->curr()->v_U();

   if( !curr_sigma_s_cache_.GetCachedResult3Dep(result, *s, *v_L, *v_U) )
   {
      SmartPtr<Vector> sigma = s->MakeNew();

      sigma->Set(0.);
      ip_nlp_->Pd_L()->AddMSinvZ(1., *curr_slack_s_L(), *v_L, *sigma);
      DBG_PRINT_VECTOR(2, "sigma1", *sigma);
      ip_nlp_->Pd_U()->AddMSinvZ(1., *curr_slack_s_U(), *v_U, *sigma);
      DBG_PRINT_VECTOR(2, "sigma2", *sigma);

      result = ConstPtr(sigma);
      curr_sigma_s_cache_.AddCachedResult3Dep(result, *s, *v_L, *v_U);
   }

   return result;
}

Number IpoptCalculatedQuantities::curr_avrg_compl()
{
   DBG_START_METH("IpoptCalculatedQuantities::curr_avrg_compl()",
                  dbg_verbosity);

   Number result;

   SmartPtr<const Vector> x = ip_data_->curr()->x();
   SmartPtr<const Vector> s = ip_data_->curr()->s();
   SmartPtr<const Vector> z_L = ip_data_->curr()->z_L();
   SmartPtr<const Vector> z_U = ip_data_->curr()->z_U();
   SmartPtr<const Vector> v_L = ip_data_->curr()->v_L();
   SmartPtr<const Vector> v_U = ip_data_->curr()->v_U();

   std::vector<const TaggedObject*> tdeps(6);
   tdeps[0] = GetRawPtr(x);
   tdeps[1] = GetRawPtr(s);
   tdeps[2] = GetRawPtr(z_L);
   tdeps[3] = GetRawPtr(z_U);
   tdeps[4] = GetRawPtr(v_L);
   tdeps[5] = GetRawPtr(v_U);

   if( !curr_avrg_compl_cache_.GetCachedResult(result, tdeps) )
   {
      if( !trial_avrg_compl_cache_.GetCachedResult(result, tdeps) )
      {

         SmartPtr<const Vector> slack_x_L = curr_slack_x_L();
         SmartPtr<const Vector> slack_x_U = curr_slack_x_U();
         SmartPtr<const Vector> slack_s_L = curr_slack_s_L();
         SmartPtr<const Vector> slack_s_U = curr_slack_s_U();

         Index ncomps = z_L->Dim() + z_U->Dim() + v_L->Dim() + v_U->Dim();

         if( ncomps > 0 )
         {
            result = z_L->Dot(*slack_x_L);
            result += z_U->Dot(*slack_x_U);
            result += v_L->Dot(*slack_s_L);
            result += v_U->Dot(*slack_s_U);

            result /= (Number) ncomps;
         }
         else
         {
            result = 0.;
         }
      }

      curr_avrg_compl_cache_.AddCachedResult(result, tdeps);
   }

   return result;
}

Number IpoptCalculatedQuantities::trial_avrg_compl()
{
   DBG_START_METH("IpoptCalculatedQuantities::trial_avrg_compl()",
                  dbg_verbosity);

   Number result;

   SmartPtr<const Vector> x = ip_data_->trial()->x();
   SmartPtr<const Vector> s = ip_data_->trial()->s();
   SmartPtr<const Vector> z_L = ip_data_->trial()->z_L();
   SmartPtr<const Vector> z_U = ip_data_->trial()->z_U();
   SmartPtr<const Vector> v_L = ip_data_->trial()->v_L();
   SmartPtr<const Vector> v_U = ip_data_->trial()->v_U();

   std::vector<const TaggedObject*> tdeps(6);
   tdeps[0] = GetRawPtr(x);
   tdeps[1] = GetRawPtr(s);
   tdeps[2] = GetRawPtr(z_L);
   tdeps[3] = GetRawPtr(z_U);
   tdeps[4] = GetRawPtr(v_L);
   tdeps[5] = GetRawPtr(v_U);

   if( !trial_avrg_compl_cache_.GetCachedResult(result, tdeps) )
   {
      if( !curr_avrg_compl_cache_.GetCachedResult(result, tdeps) )
      {

         SmartPtr<const Vector> slack_x_L = trial_slack_x_L();
         SmartPtr<const Vector> slack_x_U = trial_slack_x_U();
         SmartPtr<const Vector> slack_s_L = trial_slack_s_L();
         SmartPtr<const Vector> slack_s_U = trial_slack_s_U();

         Index ncomps = z_L->Dim() + z_U->Dim() + v_L->Dim() + v_U->Dim();

         if( ncomps > 0 )
         {
            result = z_L->Dot(*slack_x_L);
            result += z_U->Dot(*slack_x_U);
            result += v_L->Dot(*slack_s_L);
            result += v_U->Dot(*slack_s_U);

            result /= (Number) ncomps;
         }
         else
         {
            result = 0.;
         }
      }

      trial_avrg_compl_cache_.AddCachedResult(result, tdeps);
   }

   return result;
}

void IpoptCalculatedQuantities::ComputeOptimalityErrorScaling(
   const Vector& y_c,
   const Vector& y_d,
   const Vector& z_L,
   const Vector& z_U,
   const Vector& v_L,
   const Vector& v_U,
   Number        s_max,
   Number&       s_d,
   Number&       s_c
)
{
   DBG_ASSERT(initialize_called_);

   s_c = z_L.Asum() + z_U.Asum() + v_L.Asum() + v_U.Asum();
   Number n = (z_L.Dim() + z_U.Dim() + v_L.Dim() + v_U.Dim());
   if( n == 0 )
   {
      s_c = 1.0;
   }
   else
   {
      s_c = s_c / n;
      s_c = Max(s_max, s_c) / s_max;
   }

   s_d = y_c.Asum() + y_d.Asum() + z_L.Asum() + z_U.Asum() + v_L.Asum() + v_U.Asum();
   n = (y_c.Dim() + y_d.Dim() + z_L.Dim() + z_U.Dim() + v_L.Dim() + v_U.Dim());
   if( n == 0 )
   {
      s_d = 1.0;
   }
   else
   {
      s_d = s_d / n;
      s_d = Max(s_max, s_d) / s_max;
   }
}

Number IpoptCalculatedQuantities::curr_gradBarrTDelta()
{
   DBG_START_METH("IpoptCalculatedQuantities::curr_gradBarrTDelta()",
                  dbg_verbosity);

   Number result;

   SmartPtr<const Vector> x = ip_data_->curr()->x();
   SmartPtr<const Vector> s = ip_data_->curr()->s();
   SmartPtr<const Vector> delta_x = ip_data_->delta()->x();
   SmartPtr<const Vector> delta_s = ip_data_->delta()->s();
   std::vector<const TaggedObject*> tdeps(4);
   tdeps[0] = GetRawPtr(x);
   tdeps[1] = GetRawPtr(s);
   tdeps[2] = GetRawPtr(delta_x);
   tdeps[3] = GetRawPtr(delta_s);
   Number mu = ip_data_->curr_mu();
   std::vector<Number> sdeps(1);
   sdeps[0] = mu;
   DBG_PRINT((1, "curr_mu=%e\n", mu));

   if( !curr_gradBarrTDelta_cache_.GetCachedResult(result, tdeps, sdeps) )
   {
      result = curr_grad_barrier_obj_x()->Dot(*delta_x) + curr_grad_barrier_obj_s()->Dot(*delta_s);

      curr_gradBarrTDelta_cache_.AddCachedResult(result, tdeps, sdeps);
   }
   return result;
}

bool IpoptCalculatedQuantities::IsSquareProblem() const
{
   return (ip_data_->curr()->x()->Dim() == ip_data_->curr()->y_c()->Dim());
}

Vector& IpoptCalculatedQuantities::Tmp_x()
{
   if( !IsValid(tmp_x_) )
   {
      tmp_x_ = ip_data_->curr()->x()->MakeNew();
   }
   return *tmp_x_;
}

Vector& IpoptCalculatedQuantities::Tmp_s()
{
   if( !IsValid(tmp_s_) )
   {
      tmp_s_ = ip_data_->curr()->s()->MakeNew();
   }
   return *tmp_s_;
}

Vector& IpoptCalculatedQuantities::Tmp_c()
{
   if( !IsValid(tmp_c_) )
   {
      tmp_c_ = ip_data_->curr()->y_c()->MakeNew();
   }
   return *tmp_c_;
}

Vector& IpoptCalculatedQuantities::Tmp_d()
{
   if( !IsValid(tmp_d_) )
   {
      tmp_d_ = ip_data_->curr()->y_d()->MakeNew();
   }
   return *tmp_d_;
}

Vector& IpoptCalculatedQuantities::Tmp_x_L()
{
   if( !IsValid(tmp_x_L_) )
   {
      tmp_x_L_ = ip_nlp_->x_L()->MakeNew();
   }
   return *tmp_x_L_;
}

Vector& IpoptCalculatedQuantities::Tmp_x_U()
{
   if( !IsValid(tmp_x_U_) )
   {
      tmp_x_U_ = ip_nlp_->x_U()->MakeNew();
   }
   return *tmp_x_U_;
}

Vector& IpoptCalculatedQuantities::Tmp_s_L()
{
   if( !IsValid(tmp_s_L_) )
   {
      tmp_s_L_ = ip_nlp_->d_L()->MakeNew();
   }
   return *tmp_s_L_;
}

Vector& IpoptCalculatedQuantities::Tmp_s_U()
{
   if( !IsValid(tmp_s_U_) )
   {
      tmp_s_U_ = ip_nlp_->d_U()->MakeNew();
   }
   return *tmp_s_U_;
}

} // namespace Ipopt
